TEST( Transform, planeTransformation )
{
// construct a transformation we'll use for testing
Transform trans;
Matrix transformMtx;
{
Matrix m1, m2;
EulerAngles angles;
angles.set( FastFloat::fromFloat( 45.0f ), FastFloat::fromFloat( 60.0f ), FastFloat::fromFloat( -10.0f ) );
m1.setTranslation( Vector( 1, 2, 3 ) );
m2.setRotation( angles );
transformMtx.setMul( m2, m1 );
trans.set( transformMtx );
}
// create a test plane
Plane testPlane;
testPlane.set( Float_1, Float_0, Float_0, FastFloat::fromFloat( 10.0f ) );
// transform the plane
Plane tamyTransformedPlane;
trans.transform( testPlane, tamyTransformedPlane );
// let DX help us generate a transformed plane we can compare our results against
D3DXPLANE dxTransformedPlane;
D3DXMATRIX dxMtx = ( const D3DXMATRIX& )transformMtx;
D3DXMatrixInverse( &dxMtx, NULL, &dxMtx );
D3DXMatrixTranspose( &dxMtx, &dxMtx );
D3DXPlaneTransform( &dxTransformedPlane, ( const D3DXPLANE* )&testPlane, &dxMtx );
COMPARE_PLANE( dxTransformedPlane, tamyTransformedPlane );
}
/// \param angles The orientation (unit) vector that you want to reflect
/// \return Euler angles that are reflected about the slope of the plane.
EulerAngles Plane::reflectOrientation(const EulerAngles& angles) const
{
EulerAngles out;
Vector3 look, up, right;
RotationMatrix rotMat;
// get look and up vector from euler angles
rotMat.setup(angles);
look = rotMat.objectToInertial(Vector3(0.0f,0.0f,1.0f));
up = rotMat.objectToInertial(Vector3(0.0f,1.0f,0.0f));
// reflect the look and up vectors over the plane
look = reflectOrientation(look);
up = -reflectOrientation(up);
// calculate right vector
right = Vector3::crossProduct( up, look);
right.normalize();
// create a rotation matrix from right, up, and look
rotMat.m11 = right.x; rotMat.m12 = right.y; rotMat.m13 = right.z;
rotMat.m21 = up.x; rotMat.m22 = up.y; rotMat.m23 = up.z;
rotMat.m31 = look.x; rotMat.m32 = look.y; rotMat.m33 = look.z;
// calculate new euler angles from the matrix
out.fromRotationMatrix(rotMat);
return out;
}
/// rotation specified by Euler angles
Transformation Transformation::rotation(const EulerAngles& angles)
{
Transformation result = Transformation::rotation(Vector3d(0,0,1), angles.phi()) *
Transformation::rotation(Vector3d(0,1,0), angles.theta()) *
Transformation::rotation(Vector3d(1,0,0), angles.psi());
return result;
}
TEST_F(QuaternionTest, ToEulerAngles) {
set(0.966, 0.259, 0, 0);
EulerAngles ea = quaternion.uprightToEulerAngles();
ASSERT_THAT(ea.heading(), FloatNear(0, EA_DELTA));
ASSERT_THAT(ea.pitch(), FloatNear(30, EA_DELTA));
ASSERT_THAT(ea.bank(), FloatNear(0, EA_DELTA));
}
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;
}
//
//#############################################################################
//#############################################################################
//
bool
EulerAngles::TestClass()
{
SPEW((GROUP_STUFF_TEST, "Starting EulerAngle test..."));
const EulerAngles
a(Identity);
EulerAngles
b;
const EulerAngles
c(Pi_Over_4,Pi_Over_6,Pi_Over_3);
Test_Assumption(!a.pitch && !a.yaw && !a.roll);
Test_Assumption(c.pitch == Pi_Over_4 && c.yaw == Pi_Over_6 && c.roll == Pi_Over_3);
Test_Assumption(!a);
b = c;
Test_Assumption(b == c);
Test_Assumption(b != a);
Test_Assumption(b[Y_Axis] == b.yaw);
Test_Assumption(c[Z_Axis] == c.roll);
b.Lerp(a,c,0.5f);
Test_Assumption(b == EulerAngles(Stuff::Lerp(a.pitch,c.pitch,0.5f),Stuff::Lerp(a.yaw,c.yaw,0.5f),Stuff::Lerp(a.roll,c.roll,0.5f)));
LinearMatrix4D m;
m.BuildRotation(c);
b = m;
Test_Assumption(b == c);
return true;
}
void Rotation::setRotation(EulerAngles const& _angles) {
{
QSignalBlocker blocker(this);
x_->setValue(_angles.roll().degrees());
y_->setValue(_angles.pitch().degrees());
z_->setValue(_angles.yaw().degrees());
}
emit rotationChanged();
}
void
RandomEulerAngleProvider::initialize()
{
EulerAngles angle;
auto grain_num = _grain_tracker.getTotalFeatureCount();
for (auto i = _angles.size(); i < grain_num; ++i)
{
angle.random(_random);
_angles.push_back(angle);
}
}
// Routine to convert the Euler angle specifying a rotation to a quaternion.
Quaternion eulerAnglesToQuaternion(EulerAngles e)
{
float alpha, beta, gamma;
Quaternion *q1, *q2, *q3;
alpha = e.getAlpha(); beta = e.getBeta(); gamma = e.getGamma();
q1 = new Quaternion( cos( (PI/180.0) * (alpha/2.0) ), sin( (PI/180.0) * (alpha/2.0) ), 0.0, 0.0 );
q2 = new Quaternion( cos( (PI/180.0) * (beta/2.0) ), 0.0, sin( (PI/180.0) * (beta/2.0) ), 0.0 );
q3 = new Quaternion( cos( (PI/180.0) * (gamma/2.0) ), 0.0, 0.0, sin( (PI/180.0) * (gamma/2.0) ) );
return multiplyQuaternions( *q1, multiplyQuaternions(*q2, *q3) );
}
bool GlareSensor_Impl::setTransformation(const openstudio::Transformation& transformation)
{
Vector3d translation = transformation.translation();
this->setPositionXCoordinate(translation.x());
this->setPositionYCoordinate(translation.y());
this->setPositionZCoordinate(translation.z());
EulerAngles eulerAngles = transformation.eulerAngles();
setPsiRotationAroundXAxis(radToDeg(eulerAngles.psi()));
setThetaRotationAroundYAxis(radToDeg(eulerAngles.theta()));
setPhiRotationAroundZAxis(radToDeg(eulerAngles.phi()));
return true;
}
Quaternion::Quaternion(const EulerAngles &euler) :
Vector(4)
{
double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2);
setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2 * sinTheta_2 * sinPsi_2);
setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2 * cosTheta_2 * sinPsi_2);
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
sinPhi_2 * sinTheta_2 * cosPsi_2);
}
bool PlanarSurfaceGroup_Impl::setTransformation(const openstudio::Transformation& transformation) {
EulerAngles eulerAngles = transformation.eulerAngles();
if ((eulerAngles.psi() != 0) || (eulerAngles.theta() != 0)){
return false;
}
double dORN = -radToDeg(eulerAngles.phi());
this->setDirectionofRelativeNorth(dORN, false);
Vector3d translation = transformation.translation();
this->setXOrigin(translation.x(), false);
this->setYOrigin(translation.y(), false);
this->setZOrigin(translation.z(), false);
this->emitChangeSignals();
return true;
}
bool IlluminanceMap_Impl::setTransformation(const openstudio::Transformation& transformation)
{
bool test;
Vector3d translation = transformation.translation();
this->setOriginXCoordinate(translation.x());
this->setOriginYCoordinate(translation.y());
this->setOriginZCoordinate(translation.z());
EulerAngles eulerAngles = transformation.eulerAngles();
test = this->setPsiRotationAroundXAxis(radToDeg(eulerAngles.psi()));
OS_ASSERT(test);
test = this->setThetaRotationAroundYAxis(radToDeg(eulerAngles.theta()));
OS_ASSERT(test);
test = this->setPhiRotationAroundZAxis(radToDeg(eulerAngles.phi()));
OS_ASSERT(test);
return true;
}
RankFourTensor
GrainTrackerElasticity::newGrain(unsigned int new_grain_id)
{
EulerAngles angles;
if (new_grain_id < _euler.getGrainNum())
angles = _euler.getEulerAngles(new_grain_id);
else
{
if (_random_rotations)
angles.random();
else
mooseError("GrainTrackerElasticity has run out of grain rotation data.");
}
RankFourTensor C_ijkl = _C_ijkl;
C_ijkl.rotate(RotationTensor(RealVectorValue(angles)));
return C_ijkl;
}
void test(string videoName) {
const int rate = 100;
ifstream dataFile((videoName + "/TXT_" + videoName + ".txt").c_str());
int start = 0;
double tmp, frames;
dataFile >> frames >> tmp;
for (int i = 0; i < frames; ++i) {
dataFile >> tmp;
if (!start)
start = tmp;
dataFile >> tmp;
}
//vector<Quaternion> q;
int i = 0;
Quaternion p;
p.identity();
while (!dataFile.eof()) {
dataFile >> tmp;
double x, y, z;
dataFile >> x >> y >> z;
EulerAngles eulerAngles(y / rate, x / rate, z / rate);
Quaternion tq;
tq.setToRotateInertialToObject(eulerAngles);
if (i % (100 / rate) == 0)
p = p * tq;
i++;
EulerAngles e;
e.fromInertialToObjectQuaternion(p);
cout << tmp - start << " " << e.pitch << " " << e.heading << " " << e.bank << endl;
}
}
TEST( MatrixInterpolator, rotation )
{
Matrix start;
Matrix end;
start.setIdentity();
EulerAngles ea;
ea.set( FastFloat::fromFloat( 90.0f ), Float_0, Float_0 );
end.setRotation( ea );
Matrix result;
MatrixUtils::lerp( start, end, Float_0, result );
COMPARE_MTX( start, result );
MatrixUtils::lerp( start, end, Float_1, result );
COMPARE_MTX( end, result );
Matrix expectedResult;
ea.set( FastFloat::fromFloat( 45.0f ), Float_0, Float_0 );
expectedResult.setRotation( ea );
MatrixUtils::lerp( start, end, Float_Inv2, result );
COMPARE_MTX(expectedResult, result );
}
Quaternion::Quaternion(const EulerAngles &euler) :
Vector(4)
{
float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2);
setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2 * sinTheta_2 * sinPsi_2);
setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2 * cosTheta_2 * sinPsi_2);
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
sinPhi_2 * sinTheta_2 * cosPsi_2);
}
int main(int argc, char** argv)
{
UInt32 baudRate = 115200;
Int32 maxSize = 115200;
UInt8 aBuf[maxSize];
SerialInterface sp("/dev/controller_Imu", baudRate);
signal(SIGINT, catchSig);
ros::init(argc, argv, "SubImuController");
ros::NodeHandle nh;
ros::Publisher imuAttitudePub = nh.advertise<std_msgs::Float32MultiArray>("IMU_Attitude", 1000);
ros::Publisher imuAccelPub = nh.advertise<std_msgs::Float32MultiArray>("IMU_Accel_Debug", 1000);
ros::Publisher imuGyroPub = nh.advertise<std_msgs::Float32MultiArray>("IMU_Gyro_Debug", 1000);
ros::Rate loop_rate(1);
//TODO redo the whole reading and writing
while (ros::ok() && m_running)
{
int size = waitForGoodHeader(sp, aBuf);
if (size > 0)
{
UInt8 expectedSize = MipPacket::MIP_PACKET_HEADER_SIZE + MipPacket::MIP_PACKET_FOOTER_SIZE + aBuf[MipPacket::PAYLOAD_LENGTH_OFFSET]; //size of a AHRS
//printf("Expecting packet of size: %d\n",expectedSize);
while (size < expectedSize && ros::ok() && m_running)
{
int tmpSize = sp.recv(&aBuf[size],1);
if (tmpSize > 0)
{
size += tmpSize;
}
else if (tmpSize < 0)
{
printf("Error\n");
}
}
if (size == expectedSize)
{
if (aBuf[MipPacket::DESCRIPTOR_OFFSET] == 0x80)
{
MipPacket packet;
packet.deserialize(aBuf, size);
for (MipFieldNode* pIt = packet.getIterator(); pIt != NULL; pIt = pIt->m_pNext)
{
if (pIt->m_pData->getFieldDescriptor() == DataField::DATA_FIELD_EULER_ANGLES_SET)
{
EulerAngles* pData = static_cast<EulerAngles*>(pIt->m_pData);
std_msgs::Float32MultiArray rawMsg;
rawMsg.data.push_back(pData->getYaw()*(180.0/M_PI));
rawMsg.data.push_back(pData->getPitch()*(180.0/M_PI));
rawMsg.data.push_back(pData->getRoll()*(180.0/M_PI));
imuAttitudePub.publish(rawMsg);
}
else if (pIt->m_pData->getFieldDescriptor() == DataField::DATA_FIELD_SCALED_ACCELEROMETER_VECTOR_SET)
{
ScaledAccelerometerVector* pData = static_cast<ScaledAccelerometerVector*>(pIt->m_pData);
std_msgs::Float32MultiArray rawMsg;
rawMsg.data.push_back(pData->getX());
rawMsg.data.push_back(pData->getY());
rawMsg.data.push_back(pData->getZ());
imuAccelPub.publish(rawMsg);
}
else if (pIt->m_pData->getFieldDescriptor() == DataField::DATA_FIELD_SCALED_GYRO_VECTOR_SET)
{
ScaledGyroVector* pData = static_cast<ScaledGyroVector*>(pIt->m_pData);
std_msgs::Float32MultiArray rawMsg;
rawMsg.data.push_back(pData->getX());
rawMsg.data.push_back(pData->getY());
rawMsg.data.push_back(pData->getZ());
imuGyroPub.publish(rawMsg);
}
}
}
}
else
{
//printf("bad packet\n");
}
}
}
}
// Euler angles slerp
template<> EulerAngles<float> lerp(float t, const EulerAngles<float>& a, const EulerAngles<float>& b)
{
EulerAngles<float> res;
res.coeffs() = lerp(t, a.coeffs(), b.coeffs());
return res;
}
TEST_F(QuaternionTest, ObjectQuaternionFromEulerAngles) {
set(0.966, 0.259, 0, 0);
EulerAngles ea = quaternion.objectToEulerAngles();
Quaternion q = ea.toObjectQuaternion();
assertClose(q, 0.966, 0.259, 0, 0);
}
TEST_F(QuaternionTest, UprightQuaternionFromEulerAngles) {
set(0.966, 0.259, 0, 0);
EulerAngles ea = quaternion.uprightToEulerAngles();
Quaternion q = ea.toUprightQuaternion();
assertClose(q, 0.966, 0.259, 0, 0);
}
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);
}
int testRotation() {
std::cout << "\n************************************************************************\n "
<< " Rotation and Transformation Tests"
<< "\n************************************************************************\n";
std::cout << "Test Vector Rotations : ";
ok = 0;
XYZPoint v(1.,2,3.);
double pi = TMath::Pi();
// initiate rotation with some non -trivial angles to test all matrix
EulerAngles r1( pi/2.,pi/4., pi/3 );
Rotation3D r2(r1);
// only operator= is in CINT for the other rotations
Quaternion r3; r3 = r2;
AxisAngle r4; r4 = r3;
RotationZYX r5; r5 = r2;
XYZPoint v1 = r1 * v;
XYZPoint v2 = r2 * v;
XYZPoint v3 = r3 * v;
XYZPoint v4 = r4 * v;
XYZPoint v5 = r5 * v;
ok+= compare(v1.X(), v2.X(), "x",2);
ok+= compare(v1.Y(), v2.Y(), "y",2);
ok+= compare(v1.Z(), v2.Z(), "z",2);
ok+= compare(v1.X(), v3.X(), "x",2);
ok+= compare(v1.Y(), v3.Y(), "y",2);
ok+= compare(v1.Z(), v3.Z(), "z",2);
ok+= compare(v1.X(), v4.X(), "x",5);
ok+= compare(v1.Y(), v4.Y(), "y",5);
ok+= compare(v1.Z(), v4.Z(), "z",5);
ok+= compare(v1.X(), v5.X(), "x",2);
ok+= compare(v1.Y(), v5.Y(), "y",2);
ok+= compare(v1.Z(), v5.Z(), "z",2);
// test with matrix
double rdata[9];
r2.GetComponents(rdata, rdata+9);
TMatrixD m(3,3,rdata);
double vdata[3];
v.GetCoordinates(vdata);
TVectorD q(3,vdata);
TVectorD q2 = m*q;
XYZPoint v6;
v6.SetCoordinates( q2.GetMatrixArray() );
ok+= compare(v1.X(), v6.X(), "x");
ok+= compare(v1.Y(), v6.Y(), "y");
ok+= compare(v1.Z(), v6.Z(), "z");
if (ok == 0) std::cout << "\t OK " << std::endl;
else std::cout << std::endl;
std::cout << "Test Axial Rotations : ";
ok = 0;
RotationX rx( pi/3);
RotationY ry( pi/4);
RotationZ rz( 4*pi/5);
Rotation3D r3x(rx);
Rotation3D r3y(ry);
Rotation3D r3z(rz);
Quaternion qx; qx = rx;
Quaternion qy; qy = ry;
Quaternion qz; qz = rz;
RotationZYX rzyx( rz.Angle(), ry.Angle(), rx.Angle() );
XYZPoint vrot1 = rx * ry * rz * v;
XYZPoint vrot2 = r3x * r3y * r3z * v;
ok+= compare(vrot1.X(), vrot2.X(), "x");
ok+= compare(vrot1.Y(), vrot2.Y(), "y");
ok+= compare(vrot1.Z(), vrot2.Z(), "z");
vrot2 = qx * qy * qz * v;
ok+= compare(vrot1.X(), vrot2.X(), "x",2);
ok+= compare(vrot1.Y(), vrot2.Y(), "y",2);
ok+= compare(vrot1.Z(), vrot2.Z(), "z",2);
vrot2 = rzyx * v;
ok+= compare(vrot1.X(), vrot2.X(), "x");
ok+= compare(vrot1.Y(), vrot2.Y(), "y");
ok+= compare(vrot1.Z(), vrot2.Z(), "z");
// now inverse (first x then y then z)
vrot1 = rz * ry * rx * v;
vrot2 = r3z * r3y * r3x * v;
ok+= compare(vrot1.X(), vrot2.X(), "x");
ok+= compare(vrot1.Y(), vrot2.Y(), "y");
ok+= compare(vrot1.Z(), vrot2.Z(), "z");
XYZPoint vinv1 = rx.Inverse()*ry.Inverse()*rz.Inverse()*vrot1;
ok+= compare(vinv1.X(), v.X(), "x",2);
ok+= compare(vinv1.Y(), v.Y(), "y");
ok+= compare(vinv1.Z(), v.Z(), "z");
if (ok == 0) std::cout << "\t OK " << std::endl;
else std::cout << std::endl;
std::cout << "Test Rotations by a PI angle : ";
ok = 0;
double b[4] = { 6,8,10,3.14159265358979323 };
AxisAngle arPi(b,b+4 );
Rotation3D rPi(arPi);
AxisAngle a1; a1 = rPi;
ok+= compare(arPi.Axis().X(), a1.Axis().X(),"x");
ok+= compare(arPi.Axis().Y(), a1.Axis().Y(),"y");
ok+= compare(arPi.Axis().Z(), a1.Axis().Z(),"z");
ok+= compare(arPi.Angle(), a1.Angle(),"angle");
EulerAngles ePi; ePi=rPi;
EulerAngles e1; e1=Rotation3D(a1);
ok+= compare(ePi.Phi(), e1.Phi(),"phi");
ok+= compare(ePi.Theta(), e1.Theta(),"theta");
ok+= compare(ePi.Psi(), e1.Psi(),"ps1");
if (ok == 0) std::cout << "\t\t OK " << std::endl;
else std::cout << std::endl;
std::cout << "Test Inversions : ";
ok = 0;
EulerAngles s1 = r1.Inverse();
Rotation3D s2 = r2.Inverse();
Quaternion s3 = r3.Inverse();
AxisAngle s4 = r4.Inverse();
RotationZYX s5 = r5.Inverse();
// euler angles not yet impl.
XYZPoint p = s2 * r2 * v;
ok+= compare(p.X(), v.X(), "x",10);
ok+= compare(p.Y(), v.Y(), "y",10);
ok+= compare(p.Z(), v.Z(), "z",10);
p = s3 * r3 * v;
ok+= compare(p.X(), v.X(), "x",10);
ok+= compare(p.Y(), v.Y(), "y",10);
ok+= compare(p.Z(), v.Z(), "z",10);
p = s4 * r4 * v;
// axis angle inversion not very precise
ok+= compare(p.X(), v.X(), "x",1E9);
ok+= compare(p.Y(), v.Y(), "y",1E9);
ok+= compare(p.Z(), v.Z(), "z",1E9);
p = s5 * r5 * v;
ok+= compare(p.X(), v.X(), "x",10);
ok+= compare(p.Y(), v.Y(), "y",10);
ok+= compare(p.Z(), v.Z(), "z",10);
Rotation3D r6(r5);
Rotation3D s6 = r6.Inverse();
p = s6 * r6 * v;
ok+= compare(p.X(), v.X(), "x",10);
ok+= compare(p.Y(), v.Y(), "y",10);
ok+= compare(p.Z(), v.Z(), "z",10);
if (ok == 0) std::cout << "\t OK " << std::endl;
else std::cout << std::endl;
// test Rectify
std::cout << "Test rectify : ";
ok = 0;
XYZVector u1(0.999498,-0.00118212,-0.0316611);
XYZVector u2(0,0.999304,-0.0373108);
XYZVector u3(0.0316832,0.0372921,0.998802);
Rotation3D rr(u1,u2,u3);
// check orto-normality
XYZPoint vrr = rr* v;
ok+= compare(v.R(), vrr.R(), "R",1.E9);
if (ok == 0) std::cout << "\t\t OK " << std::endl;
else std::cout << std::endl;
std::cout << "Test Transform3D : ";
ok = 0;
XYZVector d(1.,-2.,3.);
Transform3D t(r2,d);
XYZPoint pd = t * v;
// apply directly rotation
XYZPoint vd = r2 * v + d;
ok+= compare(pd.X(), vd.X(), "x");
ok+= compare(pd.Y(), vd.Y(), "y");
ok+= compare(pd.Z(), vd.Z(), "z");
// test with matrix
double tdata[12];
t.GetComponents(tdata);
TMatrixD mt(3,4,tdata);
double vData[4]; // needs a vector of dim 4
v.GetCoordinates(vData);
vData[3] = 1;
TVectorD q0(4,vData);
TVectorD qt = mt*q0;
ok+= compare(pd.X(), qt(0), "x");
ok+= compare(pd.Y(), qt(1), "y");
ok+= compare(pd.Z(), qt(2), "z");
// test inverse
Transform3D tinv = t.Inverse();
p = tinv * t * v;
ok+= compare(p.X(), v.X(), "x",10);
ok+= compare(p.Y(), v.Y(), "y",10);
ok+= compare(p.Z(), v.Z(), "z",10);
// test costruct inverse from translation first
//Transform3D tinv2( -d, r2.Inverse() );
//Transform3D tinv2 = r2.Inverse() * Translation3D(-d) ;
Transform3D tinv2 ( r2.Inverse(), r2.Inverse() *( -d) ) ;
p = tinv2 * t * v;
ok+= compare(p.X(), v.X(), "x",10);
ok+= compare(p.Y(), v.Y(), "y",10);
ok+= compare(p.Z(), v.Z(), "z",10);
// test from only rotation and only translation
Transform3D ta( EulerAngles(1.,2.,3.) );
Transform3D tb( XYZVector(1,2,3) );
Transform3D tc( Rotation3D(EulerAngles(1.,2.,3.)) , XYZVector(1,2,3) );
Transform3D td( ta.Rotation(), ta.Rotation() * XYZVector(1,2,3) ) ;
ok+= compare( tc == tb*ta, static_cast<double>(true), "== Rot*Tra");
ok+= compare( td == ta*tb, static_cast<double>(true), "== Rot*Tra");
if (ok == 0) std::cout << "\t OK " << std::endl;
else std::cout << std::endl;
std::cout << "Test Plane3D : ";
ok = 0;
// test transfrom a 3D plane
XYZPoint p1(1,2,3);
XYZPoint p2(-2,-1,4);
XYZPoint p3(-1,3,2);
Plane3D plane(p1,p2,p3);
XYZVector n = plane.Normal();
// normal is perpendicular to vectors on the planes obtained from subracting the points
ok+= compare(n.Dot(p2-p1), 0.0, "n.v12",10);
ok+= compare(n.Dot(p3-p1), 0.0, "n.v13",10);
ok+= compare(n.Dot(p3-p2), 0.0, "n.v23",10);
Plane3D plane1 = t(plane);
// transform the points
XYZPoint pt1 = t(p1);
XYZPoint pt2 = t(p2);
XYZPoint pt3 = t(p3);
Plane3D plane2(pt1,pt2,pt3);
XYZVector n1 = plane1.Normal();
XYZVector n2 = plane2.Normal();
ok+= compare(n1.X(), n2.X(), "a",10);
ok+= compare(n1.Y(), n2.Y(), "b",10);
ok+= compare(n1.Z(), n2.Z(), "c",10);
ok+= compare(plane1.HesseDistance(), plane2.HesseDistance(), "d",10);
// check distances
ok += compare(plane1.Distance(pt1), 0.0, "distance",10);
if (ok == 0) std::cout << "\t OK " << std::endl;
else std::cout << std::endl;
std::cout << "Test LorentzRotation : ";
ok = 0;
XYZTVector lv(1.,2.,3.,4.);
// test from rotx (using boosts and 3D rotations not yet impl.)
// rx,ry and rz already defined
Rotation3D r3d = rx*ry*rz;
LorentzRotation rlx(rx);
LorentzRotation rly(ry);
LorentzRotation rlz(rz);
LorentzRotation rl0 = rlx*rly*rlz;
LorentzRotation rl1( r3d);
// cout << rl << endl;
// cout << rl0 << endl;
// int eq = rl0 == rl;
// cout << eq << endl;
// double d1[16];
// double d2[16];
// rl.GetComponents(d1,d1+16);
// rl0.GetComponents(d2,d2+16);
// for (int i = 0; i < 16; ++i)
// ok+= compare(d1[i], d2[i], "i",1);
//ok+= compare( rl == rl2, static_cast<double>(true), " LorenzRot");
// cout << Rotation3D(rx) << endl;
XYZTVector lv0 = rl0 * lv;
XYZTVector lv1 = rl1 * lv;
XYZTVector lv2 = r3d * lv;
ok+= compare(lv1== lv2,true,"V0==V2");
ok+= compare(lv1== lv2,true,"V1==V2");
double rlData[16];
rl0.GetComponents(rlData);
TMatrixD ml(4,4,rlData);
// ml.Print();
double lvData[4];
lv.GetCoordinates(lvData);
TVectorD ql(4,lvData);
TVectorD qlr = ml*ql;
ok+= compare(lv1.X(), qlr(0), "x");
ok+= compare(lv1.Y(), qlr(1), "y");
ok+= compare(lv1.Z(), qlr(2), "z");
ok+= compare(lv1.E(), qlr(3), "t");
// test inverse
lv0 = rl0 * rl0.Inverse() * lv;
ok+= compare(lv0.X(), lv.X(), "x");
ok+= compare(lv0.Y(), lv.Y(), "y");
ok+= compare(lv0.Z(), lv.Z(), "z");
ok+= compare(lv0.E(), lv.E(), "t");
if (ok == 0) std::cout << "\t OK " << std::endl;
else std::cout << std::endl;
// test Boosts
std::cout << "Test Boost : ";
ok = 0;
Boost bst( 0.3,0.4,0.5); // boost (must be <= 1)
XYZTVector lvb = bst ( lv );
LorentzRotation rl2 (bst);
XYZTVector lvb2 = rl2 (lv);
// test with lorentz rotation
ok+= compare(lvb.X(), lvb2.X(), "x");
ok+= compare(lvb.Y(), lvb2.Y(), "y");
ok+= compare(lvb.Z(), lvb2.Z(), "z");
ok+= compare(lvb.E(), lvb2.E(), "t");
ok+= compare(lvb.M(), lv.M(), "m",50); // m must stay constant
// test inverse
lv0 = bst.Inverse() * lvb;
ok+= compare(lv0.X(), lv.X(), "x",5);
ok+= compare(lv0.Y(), lv.Y(), "y",5);
ok+= compare(lv0.Z(), lv.Z(), "z",3);
ok+= compare(lv0.E(), lv.E(), "t",3);
XYZVector brest = lv.BoostToCM();
bst.SetComponents( brest.X(), brest.Y(), brest.Z() );
XYZTVector lvr = bst * lv;
ok+= compare(lvr.X(), 0.0, "x",10);
ok+= compare(lvr.Y(), 0.0, "y",10);
ok+= compare(lvr.Z(), 0.0, "z",10);
ok+= compare(lvr.M(), lv.M(), "m",10);
if (ok == 0) std::cout << "\t OK " << std::endl;
else std::cout << std::endl;
return ok;
}
FloatVector3 CalculateUnitViewDirVector() const { return m_heading.CalculateUnitDirectionVector(); }
