00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00057
00058 static const char rcsid[] = "$Id: homogen.cpp,v 1.15 2006/11/15 18:35:17 gourdeau Exp $";
00059
00060 #include "utils.h"
00061
00062 #ifdef use_namespace
00063 namespace ROBOOP {
00064 using namespace NEWMAT;
00065 #endif
00066
00067
00068 ReturnMatrix trans(const ColumnVector & a)
00070 {
00071 Matrix translation(4,4);
00072
00073 translation << fourbyfourident;
00074
00075 if (a.Nrows() == 3)
00076 {
00077 translation(1,4) = a(1);
00078 translation(2,4) = a(2);
00079 translation(3,4) = a(3);
00080 }
00081 else
00082 cerr << "trans: wrong size in input vector." << endl;
00083
00084 translation.Release(); return translation;
00085 }
00086
00087 ReturnMatrix rotx(const Real alpha)
00089 {
00090 Matrix rot(4,4);
00091 Real c, s;
00092
00093 rot << fourbyfourident;
00094
00095 c = cos(alpha);
00096 s = sin(alpha);
00097
00098 rot(2,2) = c;
00099 rot(3,3) = c;
00100 rot(2,3) = -s;
00101 rot(3,2) = s;
00102
00103 rot.Release(); return rot;
00104 }
00105
00106
00107 ReturnMatrix roty(const Real beta)
00109 {
00110 Matrix rot(4,4);
00111 Real c, s;
00112
00113 rot << fourbyfourident;
00114
00115 c = cos(beta);
00116 s = sin(beta);
00117
00118 rot(1,1) = c;
00119 rot(3,3) = c;
00120 rot(1,3) = s;
00121 rot(3,1) = -s;
00122
00123 rot.Release(); return rot;
00124 }
00125
00126
00127 ReturnMatrix rotz(const Real gamma)
00129 {
00130 Matrix rot(4,4);
00131 Real c, s;
00132
00133 rot << fourbyfourident;
00134
00135 c = cos(gamma);
00136 s = sin(gamma);
00137
00138 rot(1,1) = c;
00139 rot(2,2) = c;
00140 rot(1,2) = -s;
00141 rot(2,1) = s;
00142
00143 rot.Release(); return rot;
00144 }
00145
00146
00147
00148
00149 ReturnMatrix rotk(const Real theta, const ColumnVector & k)
00151 {
00152 Matrix rot(4,4);
00153 Real c, s, vers, kx, ky, kz;
00154
00155 rot << fourbyfourident;
00156
00157 vers = SumSquare(k.SubMatrix(1,3,1,1));
00158 if (vers != 0.0) {
00159 vers = sqrt(1/vers);
00160 kx = k(1)*vers;
00161 ky = k(2)*vers;
00162 kz = k(3)*vers;
00163 s = sin(theta);
00164 c = cos(theta);
00165 vers = 1-c;
00166
00167 rot(1,1) = kx*kx*vers+c;
00168 rot(1,2) = kx*ky*vers-kz*s;
00169 rot(1,3) = kx*kz*vers+ky*s;
00170 rot(2,1) = kx*ky*vers+kz*s;
00171 rot(2,2) = ky*ky*vers+c;
00172 rot(2,3) = ky*kz*vers-kx*s;
00173 rot(3,1) = kx*kz*vers-ky*s;
00174 rot(3,2) = ky*kz*vers+kx*s;
00175 rot(3,3) = kz*kz*vers+c;
00176 }
00177
00178 rot.Release(); return rot;
00179 }
00180
00181
00182 ReturnMatrix rpy(const ColumnVector & a)
00184 {
00185 Matrix rot(4,4);
00186 Real ca, sa, cb, sb, cc, sc;
00187
00188 rot << fourbyfourident;
00189
00190 ca = cos(a(1));
00191 sa = sin(a(1));
00192 cb = cos(a(2));
00193 sb = sin(a(2));
00194 cc = cos(a(3));
00195 sc = sin(a(3));
00196
00197 rot(1,1) = cb*cc;
00198 rot(1,2) = sa*sb*cc-ca*sc;
00199 rot(1,3) = ca*sb*cc+sa*sc;
00200 rot(2,1) = cb*sc;
00201 rot(2,2) = sa*sb*sc+ca*cc;
00202 rot(2,3) = ca*sb*sc-sa*cc;
00203 rot(3,1) = -sb;
00204 rot(3,2) = sa*cb;
00205 rot(3,3) = ca*cb;
00206
00207 rot.Release(); return rot;
00208 }
00209
00210
00211 ReturnMatrix eulzxz(const ColumnVector & a)
00213 {
00214 Matrix rot(4,4);
00215 Real c1, s1, ca, sa, c2, s2;
00216
00217 rot << fourbyfourident;
00218
00219 c1 = cos(a(1));
00220 s1 = sin(a(1));
00221 ca = cos(a(2));
00222 sa = sin(a(2));
00223 c2 = cos(a(3));
00224 s2 = sin(a(3));
00225
00226 rot(1,1) = c1*c2-s1*ca*s2;
00227 rot(1,2) = -c1*s2-s1*ca*c2;
00228 rot(1,3) = sa*s1;
00229 rot(2,1) = s1*c2+c1*ca*s2;
00230 rot(2,2) = -s1*s2+c1*ca*c2;
00231 rot(2,3) = -sa*c1;
00232 rot(3,1) = sa*s2;
00233 rot(3,2) = sa*c2;
00234 rot(3,3) = ca;
00235
00236 rot.Release(); return rot;
00237 }
00238
00239
00240 ReturnMatrix rotd(const Real theta, const ColumnVector & k1,
00241 const ColumnVector & k2)
00243 {
00244 Matrix rot;
00245
00246 rot = trans(k1)*rotk(theta,k2-k1)*trans(-k1);
00247
00248 rot.Release(); return rot;
00249 }
00250
00251
00252
00253 ReturnMatrix irotk(const Matrix & R)
00255 {
00256 ColumnVector k(4);
00257 Real a, b, c;
00258
00259 a = (R(3,2)-R(2,3));
00260 b = (R(1,3)-R(3,1));
00261 c = (R(2,1)-R(1,2));
00262 k(4) = atan2(sqrt(a*a + b*b + c*c),(R(1,1) + R(2,2) + R(3,3)-1));
00263 k(1) = (R(3,2)-R(2,3))/(2*sin(k(4)));
00264 k(2) = (R(1,3)-R(3,1))/(2*sin(k(4)));
00265 k(3) = (R(2,1)-R(1,2))/(2*sin(k(4)));
00266
00267 k.Release(); return k;
00268 }
00269
00270
00271 ReturnMatrix irpy(const Matrix & R)
00273 {
00274 ColumnVector k(3);
00275
00276 if (R(3,1)==1) {
00277 k(1) = atan2(-R(1,2),-R(1,3));
00278 k(2) = -M_PI/2;
00279 k(3) = 0.0;
00280 } else if (R(3,1)==-1) {
00281 k(1) = atan2(R(1,2),R(1,3));
00282 k(2) = M_PI/2;
00283 k(3) = 0.0;
00284 } else {
00285 k(1) = atan2(R(3,2), R(3,3));
00286 k(2) = atan2(-R(3,1), sqrt(R(1,1)*R(1,1) + R(2,1)*R(2,1)));
00287 k(3) = atan2(R(2,1), R(1,1));
00288 }
00289
00290 k.Release(); return k;
00291 }
00292
00293
00294 ReturnMatrix ieulzxz(const Matrix & R)
00296 {
00297 ColumnVector a(3);
00298
00299 if ((R(3,3)==1) || (R(3,3)==-1)) {
00300 a(1) = 0.0;
00301 a(2) = ((R(3,3) == 1) ? 0.0 : M_PI);
00302 a(3) = atan2(R(2,1),R(1,1));
00303 } else {
00304 a(1) = atan2(R(1,3), -R(2,3));
00305 a(2) = atan2(sqrt(R(1,3)*R(1,3) + R(2,3)*R(2,3)), R(3,3));
00306 a(3) = atan2(R(3,1), R(3,2));
00307 }
00308
00309 a.Release(); return a;
00310 }
00311
00312 #ifdef use_namespace
00313 }
00314 #endif