예제 #1
0
// @returns the rotation matrix around the vector and point by the specified angle
    //the angle is in degrees
Matrix4x4 getRotMat  (const Vector4 &p, const Vector4 &v, const REAL a) {

       /* cout<<"point in: "<<p.x<<" "<<p.y<<" "<<p.z<<" "<<p.w<<endl;
        cout<<"vec in:"<<v.x<<" "<<v.y<<" "<<v.z<<" "<<v.w<<endl;
        cout<<"angle in:"<<a<<endl;*/

	Matrix4x4 trans = getTransMat(p);

	Matrix4x4 invtrans = getInvTransMat(p);


	REAL theta = atan2(v.z,v.x);
        REAL phi = -1 * atan2(v.y , sqrt(v.x*v.x + v.z*v.z));

        Matrix4x4 M1 = getRotYMat(theta);
        Matrix4x4 M2 = getRotZMat(phi);
        Matrix4x4 M2Inv = getInvRotZMat(phi);
        Matrix4x4 M1Inv = getInvRotYMat(theta);
        //Matrix4x4 M3 = getRotXMat(M_PI*a/180);
        Matrix4x4 M3 = getRotXMat(a);
	

        Matrix4x4 toReturn = trans * M1Inv * M2Inv * M3 * M2 * M1 * invtrans;



        return toReturn;
        //return trans * M1 * M2 * M3 * M2Inv * M1Inv * invtrans;

}
// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat  (const Vector4 &p, const Vector4 &v, const REAL a) {

    // [CAMTRANS] Fill this in...
    REAL theta = atan2(v.z,v.x);
    REAL lambda = a;
    REAL phi = -atan2(v.y, sqrt(v.x*v.x+v.z*v.z));

    Matrix4x4 To = getInvTransMat(p);// translate to origin

    Matrix4x4 M1 = getRotYMat(theta);
    Matrix4x4 M2 = getRotZMat(phi);
    Matrix4x4 M3 = getRotXMat(lambda);

    Matrix4x4 M1inv = getInvRotYMat(theta);
    Matrix4x4 M2inv = getInvRotZMat(phi);

    Matrix4x4 M = M1inv*M2inv*M3*M2*M1;// full rotation matrix

    Matrix4x4 ToInv = getTransMat(p);//translate back to point

    Matrix4x4 finalmat =  ToInv*M*To;//translate to origin, rotate, and then translate back to point

    return finalmat;

}
// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat  (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 = getRotXMat(a);
    return getInvTransMat(-p)*M1_inv*M2_inv*M3*M2*M1*getTransMat(-p);

}
예제 #4
0
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);

}
예제 #5
0
파일: Matrix.cpp 프로젝트: fisch0920/Milton
// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat  (const Point3 &p, const Vector3 &v, const real_t a) {
   const real_t vZ = v[2];
   const real_t vX = v[0];
   const real_t theta = atan2(vZ, vX);
   const real_t phi   = -atan2(v[1], sqrt(vX * vX + vZ * vZ));
   
   const Matrix4x4 &transToOrigin = getInvTransMat(Vector3(p[0], p[1], p[2]));
   const Matrix4x4 &A = getRotYMat(theta);
   const Matrix4x4 &B = getRotZMat(phi);
   const Matrix4x4 &C = getRotXMat(a);
   const Matrix4x4 &invA = getInvRotYMat(theta);
   const Matrix4x4 &invB = getInvRotZMat(phi);
   const Matrix4x4 &transBack = getTransMat(Vector3(p[0], p[1], p[2]));
   
   return transBack * (invA * invB * C * B * A).getTranspose() * transToOrigin;
}
예제 #6
0
// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat  (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)*getRotXMat(a)*getRotZMat(otherAngle)*getRotYMat(theta)*invTrans;
    return final;

}
예제 #7
0
// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat  (const Vector4 &p, const Vector4 &v, const REAL a) {
    // [PASS]
    REAL theta = atan2(v.z, v.x);
    REAL phi = -atan2(v.y, sqrt(v.x*v.x + v.z*v.z));

    // translate to the origin and back
    Matrix4x4 Mt = getTransMat(-p);
    Matrix4x4 Mt_1 = getInvTransMat(-p);

    Matrix4x4 M1 = getRotYMat(theta);
    Matrix4x4 M2 = getRotZMat(phi);
    Matrix4x4 M3 = getRotXMat(a);
    Matrix4x4 M1_1 = getInvRotYMat(theta);
    Matrix4x4 M2_1 = getInvRotZMat(phi);

    return Mt_1*M1_1*M2_1*M3*M2*M1*Mt;
}
예제 #8
0
// @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;
}