124 lines
3.8 KiB
C++
124 lines
3.8 KiB
C++
/*
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*
|
|
*/
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "gazebo/math/Matrix3.hh"
|
|
|
|
using namespace gazebo;
|
|
|
|
class Matrix3Test : public ::testing::Test { };
|
|
|
|
TEST_F(Matrix3Test, Matrix3)
|
|
{
|
|
{
|
|
math::Matrix3 matrix;
|
|
EXPECT_TRUE(matrix == math::Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0));
|
|
}
|
|
|
|
{
|
|
math::Matrix3 matrix(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
|
EXPECT_TRUE(matrix == math::Matrix3(1, 2, 3, 4, 5, 6, 7, 8, 9));
|
|
|
|
math::Matrix3 matrix1(matrix);
|
|
EXPECT_TRUE(matrix1 == math::Matrix3(1, 2, 3, 4, 5, 6, 7, 8, 9));
|
|
}
|
|
|
|
math::Matrix3 matrix;
|
|
matrix.SetFromAxes(math::Vector3(1, 1, 1), math::Vector3(2, 2, 2),
|
|
math::Vector3(3, 3, 3));
|
|
EXPECT_TRUE(matrix == math::Matrix3(1, 2, 3, 1, 2, 3, 1, 2, 3));
|
|
|
|
matrix.SetFromAxis(math::Vector3(1, 1, 1), M_PI);
|
|
EXPECT_TRUE(matrix == math::Matrix3(1, 2, 2, 2, 1, 2, 2, 2, 1));
|
|
|
|
matrix.SetCol(0, math::Vector3(3, 4, 5));
|
|
EXPECT_TRUE(matrix == math::Matrix3(3, 2, 2, 4, 1, 2, 5, 2, 1));
|
|
|
|
EXPECT_THROW(matrix.SetCol(3, math::Vector3(1, 1, 1)), std::string);
|
|
}
|
|
|
|
TEST_F(Matrix3Test, Multiplication)
|
|
{
|
|
{
|
|
// Multiply arbitrary matrix by zeros of different sizes
|
|
math::Matrix3 matrix(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
|
|
|
// Scalar 0
|
|
EXPECT_EQ(math::Matrix3::ZERO, matrix * 0);
|
|
EXPECT_EQ(math::Matrix3::ZERO, 0 * matrix);
|
|
|
|
// Vector3::Zero
|
|
EXPECT_EQ(math::Vector3::Zero, matrix * math::Vector3::Zero);
|
|
// left multiply with Vector3 not implemented
|
|
|
|
// Matrix3::ZERO
|
|
EXPECT_EQ(math::Matrix3::ZERO, matrix * math::Matrix3::ZERO);
|
|
EXPECT_EQ(math::Matrix3::ZERO, math::Matrix3::ZERO * matrix);
|
|
}
|
|
|
|
{
|
|
// Multiply arbitrary matrix by identity values
|
|
math::Matrix3 matrix(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
|
|
|
// scalar 1.0
|
|
EXPECT_EQ(matrix, matrix * 1.0);
|
|
EXPECT_EQ(matrix, 1.0 * matrix);
|
|
|
|
// Vector3::Unit[X|Y|Z]
|
|
EXPECT_EQ(math::Vector3(matrix[0][0], matrix[1][0], matrix[2][0]),
|
|
matrix * math::Vector3::UnitX);
|
|
EXPECT_EQ(math::Vector3(matrix[0][1], matrix[1][1], matrix[2][1]),
|
|
matrix * math::Vector3::UnitY);
|
|
EXPECT_EQ(math::Vector3(matrix[0][2], matrix[1][2], matrix[2][2]),
|
|
matrix * math::Vector3::UnitZ);
|
|
|
|
// Matrix3::IDENTITY
|
|
EXPECT_EQ(matrix, matrix * math::Matrix3::IDENTITY);
|
|
EXPECT_EQ(matrix, math::Matrix3::IDENTITY * matrix);
|
|
}
|
|
|
|
{
|
|
// Multiply arbitrary matrix by itself
|
|
math::Matrix3 matrix(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
|
math::Matrix3 matrix2(30, 36, 42,
|
|
66, 81, 96,
|
|
102, 126, 150);
|
|
|
|
EXPECT_EQ(matrix * matrix, matrix2);
|
|
}
|
|
}
|
|
|
|
TEST_F(Matrix3Test, Inverse)
|
|
{
|
|
// Inverse of identity matrix is itself
|
|
EXPECT_EQ(math::Matrix3::IDENTITY, math::Matrix3::IDENTITY.Inverse());
|
|
|
|
// Matrix multiplied by its inverse results in the identity matrix
|
|
math::Matrix3 matrix1(-2, 4, 0, 0.1, 9, 55, -7, 1, 26);
|
|
math::Matrix3 matrix2 = matrix1.Inverse();
|
|
EXPECT_EQ(matrix1 * matrix2, math::Matrix3::IDENTITY);
|
|
EXPECT_EQ(matrix2 * matrix1, math::Matrix3::IDENTITY);
|
|
|
|
// Inverse of inverse results in the same matrix
|
|
EXPECT_EQ((matrix1.Inverse()).Inverse(), matrix1);
|
|
|
|
// Invert multiplication by scalar
|
|
double scalar = 2.5;
|
|
EXPECT_EQ((matrix1 * scalar).Inverse(), matrix1.Inverse() * (1.0/scalar));
|
|
}
|