// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// @TODO: [CAMTRANS] Fill this in...
const REAL y = atan2(v.z,v.x), z = -atan2(v.y,std::sqrt(SQ(v.x)+SQ(v.z)));
const Matrix4x4 M1 = getRotYMat(y), M1_inv = getInvRotYMat(y),
M2 = getRotZMat(z), M2_inv = getInvRotZMat(z),
M3_inv = getInvRotXMat(a);
return getInvTransMat(-p)*M1_inv*M2_inv*M3_inv*M2*M1*getTransMat(-p);
}
void testMatrix::testMatrices()
{
Vector4 testTransV = Vector4(4,5,6,1);
Matrix4x4 testTrans = Matrix4x4(1,0,0,-4,0,1,0,-5,0,0,1,-6,0,0,0,1);
Matrix4x4 testRotZ90 = Matrix4x4(0.f,-1.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,1.f);
Matrix4x4 testRotY90 = Matrix4x4(0.f,0.f,1.f,0.f,0.f,1.f,0.f,0.f,-1.f,0.f,0.f,0.f,0.f,0.f,0.f,1.f);
Matrix4x4 testRotX90 = Matrix4x4(1.f,0.f,0.f,0.f,0.f,0.f,-1.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,0.f,1.f);
//y
Matrix4x4 testRot1 = getRotMat(Vector4(0,0,0,1),Vector4(0,1,0,1),90);
//x
Matrix4x4 testRot2 = getRotMat(Vector4(0,0,0,1),Vector4(1,0,0,1), 90);
//z
Matrix4x4 testRot3 = getRotMat(Vector4(0,0,0,1),Vector4(0,0,1,1), 90);
Matrix4x4 trz90 = getRotZMat(M_PI/2);
Matrix4x4 try90 = getRotYMat(M_PI/2);
Matrix4x4 trx90 = getRotXMat(M_PI/2);
Matrix4x4 trans = getTransMat(testTransV);
//Test rotation on z axis
compareMatrices(&testRotZ90,&trz90);
//Z axis as arbitrary rotation
compareMatrices(&testRotZ90,&testRot3);
//inverses are equivalent:
compareMatrices(&getInvRotZMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(0,0,1,1),90));
//Test rotation on y axis
compareMatrices(&testRotY90,&try90);
//Y axis as arbitrary rotation
compareMatrices(&testRotY90,&testRot1);
//inverses are equivalent:
compareMatrices(&getInvRotYMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(0,1,0,1),90));
//Test rotation on x axis
compareMatrices(&testRotX90,&trx90);
//X axis as arbitrary rotation
compareMatrices(&testRotX90,&testRot2);
//inverses are equivalent:
compareMatrices(&getInvRotXMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(1,0,0,1),90));
//test translation
compareMatrices(&testTrans,&trans);
}
// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// angles to rotate v to x-axis
REAL th = atan2(v.z,v.x),
ph = -atan2(v.y,sqrt(pow(v.z,2)+pow(v.x,2)));
// all transformations required
// Note: Inverse of Rot mat is just transpose
Matrix4x4 T = getInvTransMat(p),
Tinv = getTransMat(p),
M1 = getRotYMat(th),
M2 = getRotZMat(ph),
M3 = getInvRotXMat(a); // M3^-1 is only diff btw this and regular func
return Tinv * M1.getTranspose() * M2.getTranspose()
* M3 * M2 * M1 * T;
}
// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// @TODO: [CAMTRANS] Fill this in...
REAL theta = atan2(v.z, v.x);
REAL otherAngle = -atan2(v.y, sqrt(v.x*v.x+v.z*v.z));
Matrix4x4 trans = Matrix4x4(1,0,0,p.x,
0,1,0, p.y,
0,0,1, p.z,
0,0,0,1);
Matrix4x4 invTrans = Matrix4x4(1,0,0, -p.x,
0,1,0, -p.y,
0,0,1, -p.z,
0,0,0,1);
Matrix4x4 final = trans*getInvRotYMat(theta)*getInvRotZMat(otherAngle)*getInvRotXMat(a)*getRotZMat(otherAngle)*getRotYMat(theta)*invTrans;
return final;
}
// @returns the inverse rotation matrix around the vector and
// point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &h, const Vector4 &a, const REAL lambda) {
// @DONE: [CAMTRANS] Filled in.
double theta = atan2(a.z, a.x);
double phi = -atan2(a.y, sqrt(a.x*a.x + a.z*a.z));
//want to translate by -h to move from h to the origin
Matrix4x4 Th = getTransMat(-h);
Matrix4x4 M1Inv = getInvRotYMat(theta);
Matrix4x4 M2Inv = getInvRotZMat(phi);
Matrix4x4 M3Inv = getInvRotXMat(lambda);
Matrix4x4 M2 = getRotZMat(phi);
Matrix4x4 M1 = getRotYMat(theta);
Matrix4x4 ThInv = getInvTransMat(-h);
Matrix4x4 result = ThInv * M1Inv * M2Inv * M3Inv * M2 * M1 * Th;
return result;
}