bool GlareSensor_Impl::aimAt(const Point3d& target)
{
Point3d position = this->position();
Vector3d vector = target - position;
if (!vector.normalize()){
return false;
}
Vector3d yAxis(0,1,0);
Vector3d rotationAxis = yAxis.cross(vector);
if (!rotationAxis.normalize()){
return false;
}
double angle = getAngle(yAxis, vector);
Transformation transformation = Transformation::rotation(rotationAxis, angle);
EulerAngles eulerAngles = transformation.eulerAngles();
this->setPsiRotationAroundXAxis(eulerAngles.psi());
this->setThetaRotationAroundYAxis(eulerAngles.theta());
this->setPhiRotationAroundZAxis(eulerAngles.phi());
return true;
}
TEST_F(GeometryFixture, Transformation_Decompose)
{
Transformation translation = Transformation::translation(Vector3d(1, 0, 0));
Transformation rotation = Transformation::rotation(Vector3d(0,0,1), degToRad(-90));
Transformation transformation = translation*rotation;
Vector3d origin = transformation.translation();
EulerAngles angles = transformation.eulerAngles();
Transformation test = Transformation::translation(origin)*Transformation::rotation(angles);
EXPECT_TRUE(transformation.matrix() == test.matrix()) << transformation.matrix() << std::endl << test.matrix();
transformation = rotation*translation;
origin = transformation.translation();
angles = transformation.eulerAngles();
test = Transformation::translation(origin)*Transformation::rotation(angles);
EXPECT_TRUE(transformation.matrix() == test.matrix()) << transformation.matrix() << std::endl << test.matrix();
}
TEST_F(GeometryFixture, EulerAngles)
{
Transformation transformation;
EulerAngles angles = transformation.eulerAngles();
EXPECT_EQ(0.0, angles.psi());
EXPECT_EQ(0.0, angles.theta());
EXPECT_EQ(0.0, angles.phi());
angles = EulerAngles(0,0,0);
transformation = Transformation::rotation(angles);
Matrix rotationMatrix = transformation.rotationMatrix();
ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size1());
ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size2());
EXPECT_EQ(1.0, rotationMatrix(0,0));
EXPECT_EQ(0.0, rotationMatrix(0,1));
EXPECT_EQ(0.0, rotationMatrix(0,2));
EXPECT_EQ(0.0, rotationMatrix(1,0));
EXPECT_EQ(1.0, rotationMatrix(1,1));
EXPECT_EQ(0.0, rotationMatrix(1,2));
EXPECT_EQ(0.0, rotationMatrix(2,0));
EXPECT_EQ(0.0, rotationMatrix(2,1));
EXPECT_EQ(1.0, rotationMatrix(2,2));
transformation = Transformation::translation(Vector3d(1,1,1));
angles = transformation.eulerAngles();
EXPECT_EQ(0.0, angles.psi());
EXPECT_EQ(0.0, angles.theta());
EXPECT_EQ(0.0, angles.phi());
transformation = Transformation::rotation(Vector3d(1,0,0), 1.0);
angles = transformation.eulerAngles();
EXPECT_EQ(1.0, angles.psi());
EXPECT_EQ(0.0, angles.theta());
EXPECT_EQ(0.0, angles.phi());
angles = EulerAngles(1,0,0);
transformation = Transformation::rotation(angles);
rotationMatrix = transformation.rotationMatrix();
ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size1());
ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size2());
EXPECT_NEAR(1.0, rotationMatrix(0,0), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(0,1), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(0,2), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(1,0), 0.0001);
EXPECT_NEAR(cos(1.0), rotationMatrix(1,1), 0.0001);
EXPECT_NEAR(-sin(1.0), rotationMatrix(1,2), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(2,0), 0.0001);
EXPECT_NEAR(sin(1.0), rotationMatrix(2,1), 0.0001);
EXPECT_NEAR(cos(1.0), rotationMatrix(2,2), 0.0001);
transformation = Transformation::rotation(Vector3d(0,1,0), 1.0);
angles = transformation.eulerAngles();
EXPECT_EQ(0.0, angles.psi());
EXPECT_EQ(1.0, angles.theta());
EXPECT_EQ(0.0, angles.phi());
angles = EulerAngles(0,1,0);
transformation = Transformation::rotation(angles);
rotationMatrix = transformation.rotationMatrix();
ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size1());
ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size2());
EXPECT_NEAR(cos(1.0), rotationMatrix(0,0), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(0,1), 0.0001);
EXPECT_NEAR(sin(1.0), rotationMatrix(0,2), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(1,0), 0.0001);
EXPECT_NEAR(1.0, rotationMatrix(1,1), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(1,2), 0.0001);
EXPECT_NEAR(-sin(1.0), rotationMatrix(2,0), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(2,1), 0.0001);
EXPECT_NEAR(cos(1.0), rotationMatrix(2,2), 0.0001);
transformation = Transformation::rotation(Vector3d(0,0,1), 1.0);
angles = transformation.eulerAngles();
EXPECT_EQ(0.0, angles.psi());
EXPECT_EQ(0.0, angles.theta());
EXPECT_EQ(1.0, angles.phi());
angles = EulerAngles(0,0,1);
transformation = Transformation::rotation(angles);
rotationMatrix = transformation.rotationMatrix();
ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size1());
ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size2());
EXPECT_NEAR(cos(1.0), rotationMatrix(0,0), 0.0001);
EXPECT_NEAR(-sin(1.0), rotationMatrix(0,1), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(0,2), 0.0001);
EXPECT_NEAR(sin(1.0), rotationMatrix(1,0), 0.0001);
EXPECT_NEAR(cos(1.0), rotationMatrix(1,1), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(1,2), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(2,0), 0.0001);
EXPECT_NEAR(0.0, rotationMatrix(2,1), 0.0001);
EXPECT_NEAR(1.0, rotationMatrix(2,2), 0.0001);
Matrix matrix(4,4);
matrix(0,0) = 0.5;
matrix(0,1) = -0.1464;
matrix(0,2) = 0.8536;
matrix(1,0) = 0.5;
matrix(1,1) = 0.8536;
matrix(1,2) = -0.1464;
matrix(2,0) = -0.7071;
matrix(2,1) = 0.5;
matrix(2,2) = 0.5;
matrix(3,3) = 1.0;
transformation = Transformation(matrix);
angles = transformation.eulerAngles();
EXPECT_NEAR(boost::math::constants::pi<double>()/4.0, angles.psi(), 0.0001);
EXPECT_NEAR(boost::math::constants::pi<double>()/4.0, angles.theta(), 0.0001);
EXPECT_NEAR(boost::math::constants::pi<double>()/4.0, angles.phi(), 0.0001);
angles = EulerAngles(boost::math::constants::pi<double>()/4.0,
boost::math::constants::pi<double>()/4.0,
boost::math::constants::pi<double>()/4.0);
transformation = Transformation::rotation(angles);
rotationMatrix = transformation.rotationMatrix();
EXPECT_NEAR(0.5, rotationMatrix(0,0), 0.0001);
EXPECT_NEAR(-0.1464, rotationMatrix(0,1), 0.0001);
EXPECT_NEAR(0.8536, rotationMatrix(0,2), 0.0001);
EXPECT_NEAR(0.5, rotationMatrix(1,0), 0.0001);
EXPECT_NEAR(0.8536, rotationMatrix(1,1), 0.0001);
EXPECT_NEAR(-0.1464, rotationMatrix(1,2), 0.0001);
EXPECT_NEAR(-0.7071, rotationMatrix(2,0), 0.0001);
EXPECT_NEAR(0.5, rotationMatrix(2,1), 0.0001);
EXPECT_NEAR(0.5, rotationMatrix(2,2), 0.0001);
}
