Eigen4d.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
10 #include "Eigen4d.h"
11 #include "VrmlNodes.h"
12 
13 using namespace hrp;
14 
15 
16 void hrp::calcRodrigues(Matrix44& out_R, const Vector3& axis, double q)
17 {
18  const double sth = sin(q);
19  const double vth = 1.0 - cos(q);
20 
21  double ax = axis(0);
22  double ay = axis(1);
23  double az = axis(2);
24 
25  const double axx = ax*ax*vth;
26  const double ayy = ay*ay*vth;
27  const double azz = az*az*vth;
28  const double axy = ax*ay*vth;
29  const double ayz = ay*az*vth;
30  const double azx = az*ax*vth;
31 
32  ax *= sth;
33  ay *= sth;
34  az *= sth;
35 
36  out_R <<
37  1.0 - azz - ayy, -az + axy, ay + azx, 0.0,
38  az + axy, 1.0 - azz - axx, -ax + ayz, 0.0,
39  -ay + azx, ax + ayz, 1.0 - ayy - axx, 0.0,
40  0.0, 0.0, 0.0, 1.0;
41 }
42 
50 {
51  Matrix44 R;
52  const SFRotation& r = transform->rotation;
53  calcRodrigues(R, Vector3(r[0], r[1], r[2]), r[3]);
54 
55  const SFVec3f& center = transform->center;
56 
57  Matrix44 SR;
58  const SFRotation& so = transform->scaleOrientation;
59  calcRodrigues(SR, Vector3(so[0], so[1], so[2]), so[3]);
60 
61  const SFVec3f& s = transform->scale;
62 
63  Matrix44 SinvSR;
64  SinvSR <<
65  s[0] * SR(0,0), s[0] * SR(1,0), s[0] * SR(2,0), 0.0,
66  s[1] * SR(0,1), s[1] * SR(1,1), s[1] * SR(2,1), 0.0,
67  s[2] * SR(0,2), s[2] * SR(1,2), s[2] * SR(2,2), 0.0,
68  0.0, 0.0, 0.0, 1.0;
69 
70  const Vector4 c(center[0], center[1], center[2], 1.0);
71 
72  Matrix44 RSR(R * SR);
73 
74  out_T = RSR * SinvSR;
75 
76  const Vector4 c2(out_T * c);
77  for(int i=0; i < 3; ++i){
78  out_T(i, 3) -= c2(i);
79  }
80 
81  for(int i=0; i < 3; ++i){
82  out_T(i, 3) += transform->translation[i] + center[i];
83  }
84 }
int c
Definition: autoplay.py:16
HRP_UTIL_EXPORT void calcTransformMatrix(VrmlTransform *transform, Matrix44 &out_T)
Definition: Eigen4d.cpp:49
SFRotation rotation
Definition: VrmlNodes.h:260
png_uint_32 i
Definition: png.h:2735
Eigen::Matrix4d Matrix44
Definition: Eigen4d.h:18
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
SFRotation scaleOrientation
Definition: VrmlNodes.h:262
Eigen::Vector4d Vector4
Definition: Eigen4d.h:21
boost::array< SFFloat, 3 > SFVec3f
Definition: VrmlNodes.h:55
SFVec4f SFRotation
Definition: VrmlNodes.h:59
HRP_UTIL_EXPORT void calcRodrigues(Matrix33 &out_R, const Vector3 &axis, double q)
Definition: Eigen3d.cpp:22
VRML Transform node.
Definition: VrmlNodes.h:254
SFVec3f translation
Definition: VrmlNodes.h:263


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:37