Eigen4d.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
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 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53