00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include "Tvmet3d.h"
00012
00013 using namespace tvmet;
00014 using namespace hrp;
00015
00016
00017 static const double PI = 3.14159265358979323846;
00018
00019
00020 void hrp::calcRodrigues(Matrix33& out_R, const Vector3& axis, double q)
00021 {
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 const double sth = sin(q);
00038 const double vth = 1.0 - cos(q);
00039
00040 double ax = axis(0);
00041 double ay = axis(1);
00042 double az = axis(2);
00043
00044 const double axx = ax*ax*vth;
00045 const double ayy = ay*ay*vth;
00046 const double azz = az*az*vth;
00047 const double axy = ax*ay*vth;
00048 const double ayz = ay*az*vth;
00049 const double azx = az*ax*vth;
00050
00051 ax *= sth;
00052 ay *= sth;
00053 az *= sth;
00054
00055 out_R = 1.0 - azz - ayy, -az + axy, ay + azx,
00056 az + axy, 1.0 - azz - axx, -ax + ayz,
00057 -ay + azx, ax + ayz, 1.0 - ayy - axx;
00058 }
00059
00060
00061 Vector3 hrp::omegaFromRot(const Matrix33& r)
00062 {
00063 using ::std::numeric_limits;
00064
00065 double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
00066
00067 if(fabs(alpha - 1.0) < 1.0e-6) {
00068 return Vector3(0.0);
00069
00070 } else {
00071 double th = acos(alpha);
00072 double s = sin(th);
00073
00074 if (s < numeric_limits<double>::epsilon()) {
00075 return Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th );
00076 }
00077
00078 double k = -0.5 * th / s;
00079
00080 return Vector3( (r(1,2) - r(2,1)) * k,
00081 (r(2,0) - r(0,2)) * k,
00082 (r(0,1) - r(1,0)) * k );
00083 }
00084 }
00085
00086
00087 Vector3 hrp::rpyFromRot(const Matrix33& m)
00088 {
00089 double roll, pitch, yaw;
00090
00091 if ((fabs(m(0,0))<fabs(m(2,0))) && (fabs(m(1,0))<fabs(m(2,0)))) {
00092
00093 double sp = -m(2,0);
00094 if (sp < -1.0) {
00095 sp = -1;
00096 } else if (sp > 1.0) {
00097 sp = 1;
00098 }
00099 pitch = asin(sp);
00100
00101 roll = atan2(sp*m(0,1)+m(1,2),
00102 sp*m(0,2)-m(1,1));
00103
00104 if (m(0,0)>0.0) {
00105 (roll < 0.0) ? (roll += PI) : (roll -= PI);
00106 }
00107 double sr=sin(roll), cr=cos(roll);
00108 if (sp > 0.0) {
00109 yaw = atan2(sr*m(1,1)+cr*m(1,2),
00110 sr*m(0,1)+cr*m(0,2));
00111 } else {
00112 yaw = atan2(-sr*m(1,1)-cr*m(1,2),
00113 -sr*m(0,1)-cr*m(0,2));
00114 }
00115 } else {
00116 yaw = atan2(m(1,0), m(0,0));
00117 const double sa = sin(yaw);
00118 const double ca = cos(yaw);
00119 pitch = atan2(-m(2,0), ca*m(0,0)+sa*m(1,0));
00120 roll = atan2(sa*m(0,2)-ca*m(1,2), -sa*m(0,1)+ca*m(1,1));
00121 }
00122 return Vector3(roll, pitch, yaw);
00123 }
00124
00125
00126 void hrp::calcRotFromRpy(Matrix33& out_R, double r, double p, double y)
00127 {
00128 const double cr = cos(r), sr = sin(r), cp = cos(p), sp = sin(p), cy = cos(y), sy = sin(y);
00129 out_R(0,0)= cp*cy;
00130 out_R(0,1)= sr*sp*cy - cr*sy;
00131 out_R(0,2)= cr*sp*cy + sr*sy;
00132 out_R(1,0)= cp*sy;
00133 out_R(1,1)= sr*sp*sy + cr*cy;
00134 out_R(1,2)= cr*sp*sy - sr*cy;
00135 out_R(2,0)= -sp;
00136 out_R(2,1)= sr*cp;
00137 out_R(2,2)= cr*cp;
00138 }
00139
00140
00141 void hrp::calcInverse(Matrix33& inv, const Matrix33& m)
00142 {
00143 double det = m(0,0)*(m(1,1)*m(2,2)-m(1,2)*m(2,1)) - m(0,1)*(m(1,0)*m(2,2)-m(1,2)*m(2,0)) + m(0,2)*(m(1,0)*m(2,1)-m(1,1)*m(2,0));
00144
00145 if(fabs(det) < std::numeric_limits<double>::epsilon()){
00146 throw std::string("Invrse matrix cannot be calculated.");
00147 }
00148 else{
00149 inv =
00150 (m(1,1)*m(2,2)-m(1,2)*m(2,1)) / det, (m(0,2)*m(2,1)-m(0,1)*m(2,2)) / det, (m(0,1)*m(1,2)-m(0,2)*m(1,1)) / det,
00151 (m(1,2)*m(2,0)-m(1,0)*m(2,2)) / det, (m(0,0)*m(2,2)-m(0,2)*m(2,0)) / det, (m(0,2)*m(1,0)-m(0,0)*m(1,2)) / det,
00152 (m(1,0)*m(2,1)-m(1,1)*m(2,0)) / det, (m(0,1)*m(2,0)-m(0,0)*m(2,1)) / det, (m(0,0)*m(1,1)-m(0,1)*m(1,0)) / det;
00153 }
00154 }
00155
00156 bool hrp::isOrthogonalMatrix(Matrix33& m){
00157 return all_elements( m * trans(m) == tvmet::identity<Matrix33>() );
00158 }