00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include "Eigen4d.h"
00011 #include "VrmlNodes.h"
00012
00013 using namespace hrp;
00014
00015
00016 void hrp::calcRodrigues(Matrix44& out_R, const Vector3& axis, double q)
00017 {
00018 const double sth = sin(q);
00019 const double vth = 1.0 - cos(q);
00020
00021 double ax = axis(0);
00022 double ay = axis(1);
00023 double az = axis(2);
00024
00025 const double axx = ax*ax*vth;
00026 const double ayy = ay*ay*vth;
00027 const double azz = az*az*vth;
00028 const double axy = ax*ay*vth;
00029 const double ayz = ay*az*vth;
00030 const double azx = az*ax*vth;
00031
00032 ax *= sth;
00033 ay *= sth;
00034 az *= sth;
00035
00036 out_R <<
00037 1.0 - azz - ayy, -az + axy, ay + azx, 0.0,
00038 az + axy, 1.0 - azz - axx, -ax + ayz, 0.0,
00039 -ay + azx, ax + ayz, 1.0 - ayy - axx, 0.0,
00040 0.0, 0.0, 0.0, 1.0;
00041 }
00042
00049 void hrp::calcTransformMatrix(VrmlTransform* transform, Matrix44& out_T)
00050 {
00051 Matrix44 R;
00052 const SFRotation& r = transform->rotation;
00053 calcRodrigues(R, Vector3(r[0], r[1], r[2]), r[3]);
00054
00055 const SFVec3f& center = transform->center;
00056
00057 Matrix44 SR;
00058 const SFRotation& so = transform->scaleOrientation;
00059 calcRodrigues(SR, Vector3(so[0], so[1], so[2]), so[3]);
00060
00061 const SFVec3f& s = transform->scale;
00062
00063 Matrix44 SinvSR;
00064 SinvSR <<
00065 s[0] * SR(0,0), s[0] * SR(1,0), s[0] * SR(2,0), 0.0,
00066 s[1] * SR(0,1), s[1] * SR(1,1), s[1] * SR(2,1), 0.0,
00067 s[2] * SR(0,2), s[2] * SR(1,2), s[2] * SR(2,2), 0.0,
00068 0.0, 0.0, 0.0, 1.0;
00069
00070 const Vector4 c(center[0], center[1], center[2], 1.0);
00071
00072 Matrix44 RSR(R * SR);
00073
00074 out_T = RSR * SinvSR;
00075
00076 const Vector4 c2(out_T * c);
00077 for(int i=0; i < 3; ++i){
00078 out_T(i, 3) -= c2(i);
00079 }
00080
00081 for(int i=0; i < 3; ++i){
00082 out_T(i, 3) += transform->translation[i] + center[i];
00083 }
00084 }