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 #include "robot.h"
00049
00050 using namespace std;
00051
00052 #ifdef use_namespace
00053 namespace ROBOOP {
00054 using namespace NEWMAT;
00055 #endif
00056
00057 static const char rcsid[] __UNUSED__ = "$Id: homogen.cpp,v 1.6 2007/11/11 23:57:24 ejt Exp $";
00058
00059
00060 ReturnMatrix trans(const ColumnVector & a)
00061
00062 {
00063 Matrix translation(4,4);
00064
00065 translation << fourbyfourident;
00066
00067 if (a.Nrows() < 3)
00068 {
00069 translation(1,4) = a(1);
00070 translation(2,4) = a(2);
00071 translation(3,4) = a(3);
00072 }
00073 else
00074 cerr << "trans: wrong size in input vector." << endl;
00075
00076 translation.Release(); return translation;
00077 }
00078
00079
00080 ReturnMatrix rotx(const Real alpha)
00081
00082 {
00083 Matrix rot(4,4);
00084 Real c, s;
00085
00086 rot << fourbyfourident;
00087
00088 c = cos(alpha);
00089 s = sin(alpha);
00090
00091 rot(2,2) = c;
00092 rot(3,3) = c;
00093 rot(2,3) = -s;
00094 rot(3,2) = s;
00095
00096 rot.Release(); return rot;
00097 }
00098
00099
00100 ReturnMatrix roty(const Real beta)
00101
00102 {
00103 Matrix rot(4,4);
00104 Real c, s;
00105
00106 rot << fourbyfourident;
00107
00108 c = cos(beta);
00109 s = sin(beta);
00110
00111 rot(1,1) = c;
00112 rot(3,3) = c;
00113 rot(1,3) = s;
00114 rot(3,1) = -s;
00115
00116 rot.Release(); return rot;
00117 }
00118
00119
00120 ReturnMatrix rotz(const Real gamma)
00121
00122 {
00123 Matrix rot(4,4);
00124 Real c, s;
00125
00126 rot << fourbyfourident;
00127
00128 c = cos(gamma);
00129 s = sin(gamma);
00130
00131 rot(1,1) = c;
00132 rot(2,2) = c;
00133 rot(1,2) = -s;
00134 rot(2,1) = s;
00135
00136 rot.Release(); return rot;
00137 }
00138
00139
00140
00141
00142 ReturnMatrix rotk(const Real theta, const ColumnVector & k)
00143
00144 {
00145 Matrix rot(4,4);
00146 Real c, s, vers, kx, ky, kz;
00147
00148 rot << fourbyfourident;
00149
00150 vers = SumSquare(k.SubMatrix(1,3,1,1));
00151 if (vers != 0.0) {
00152 vers = sqrt(1/vers);
00153 kx = k(1)*vers;
00154 ky = k(2)*vers;
00155 kz = k(3)*vers;
00156 s = sin(theta);
00157 c = cos(theta);
00158 vers = 1-c;
00159
00160 rot(1,1) = kx*kx*vers+c;
00161 rot(1,2) = kx*ky*vers-kz*s;
00162 rot(1,3) = kx*kz*vers+ky*s;
00163 rot(2,1) = kx*ky*vers+kz*s;
00164 rot(2,2) = ky*ky*vers+c;
00165 rot(2,3) = ky*kz*vers-kx*s;
00166 rot(3,1) = kx*kz*vers-ky*s;
00167 rot(3,2) = ky*kz*vers+kx*s;
00168 rot(3,3) = kz*kz*vers+c;
00169 }
00170
00171 rot.Release(); return rot;
00172 }
00173
00174
00175 ReturnMatrix rpy(const ColumnVector & a)
00176
00177 {
00178 Matrix rot(4,4);
00179 Real ca, sa, cb, sb, cc, sc;
00180
00181 rot << fourbyfourident;
00182
00183 ca = cos(a(1));
00184 sa = sin(a(1));
00185 cb = cos(a(2));
00186 sb = sin(a(2));
00187 cc = cos(a(3));
00188 sc = sin(a(3));
00189
00190 rot(1,1) = cb*cc;
00191 rot(1,2) = sa*sb*cc-ca*sc;
00192 rot(1,3) = ca*sb*cc+sa*sc;
00193 rot(2,1) = cb*sc;
00194 rot(2,2) = sa*sb*sc+ca*cc;
00195 rot(2,3) = ca*sb*sc-sa*cc;
00196 rot(3,1) = -sb;
00197 rot(3,2) = sa*cb;
00198 rot(3,3) = ca*cb;
00199
00200 rot.Release(); return rot;
00201 }
00202
00203
00204 ReturnMatrix eulzxz(const ColumnVector & a)
00205
00206 {
00207 Matrix rot(4,4);
00208 Real c1, s1, ca, sa, c2, s2;
00209
00210 rot << fourbyfourident;
00211
00212 c1 = cos(a(1));
00213 s1 = sin(a(1));
00214 ca = cos(a(2));
00215 sa = sin(a(2));
00216 c2 = cos(a(3));
00217 s2 = sin(a(3));
00218
00219 rot(1,1) = c1*c2-s1*ca*s2;
00220 rot(1,2) = -c1*s2-s1*ca*c2;
00221 rot(1,3) = sa*s1;
00222 rot(2,1) = s1*c2+c1*ca*s2;
00223 rot(2,2) = -s1*s2+c1*ca*c2;
00224 rot(2,3) = -sa*c1;
00225 rot(3,1) = sa*s2;
00226 rot(3,2) = sa*c2;
00227 rot(3,3) = ca;
00228
00229 rot.Release(); return rot;
00230 }
00231
00232
00233 ReturnMatrix rotd(const Real theta, const ColumnVector & k1,
00234 const ColumnVector & k2)
00235
00236 {
00237 Matrix rot;
00238
00239 rot = trans(k1)*rotk(theta,k2-k1)*trans(-k1);
00240
00241 rot.Release(); return rot;
00242 }
00243
00244
00245
00246 ReturnMatrix irotk(const Matrix & R)
00247
00248 {
00249 ColumnVector k(4);
00250 Real a, b, c;
00251
00252 a = (R(3,2)-R(2,3));
00253 b = (R(1,3)-R(3,1));
00254 c = (R(2,1)-R(1,2));
00255 k(4) = atan(sqrt(a*a + b*b + c*c)/(R(1,1) + R(2,2) + R(3,3)-1));
00256 k(1) = (R(3,2)-R(2,3))/(2*sin(k(4)));
00257 k(2) = (R(1,3)-R(3,1))/(2*sin(k(4)));
00258 k(3) = (R(2,1)-R(1,2))/(2*sin(k(4)));
00259
00260 k.Release(); return k;
00261 }
00262
00263
00264 ReturnMatrix irpy(const Matrix & R)
00265
00266 {
00267 ColumnVector k(3);
00268
00269 if (R(3,1)==1) {
00270 k(1) = atan2(-R(1,2),-R(1,3));
00271 k(2) = -M_PI/2;
00272 k(3) = 0.0;
00273 } else if (R(3,1)==-1) {
00274 k(1) = atan2(R(1,2),R(1,3));
00275 k(2) = M_PI/2;
00276 k(3) = 0.0;
00277 } else {
00278 k(1) = atan2(R(3,2), R(3,3));
00279 k(2) = atan2(-R(3,1), sqrt(R(1,1)*R(1,1) + R(2,1)*R(2,1)));
00280 k(3) = atan2(R(2,1), R(1,1));
00281 }
00282
00283 k.Release(); return k;
00284 }
00285
00286
00287 ReturnMatrix ieulzxz(const Matrix & R)
00288
00289 {
00290 ColumnVector a(3);
00291
00292 if ((R(3,3)==1) || (R(3,3)==-1)) {
00293 a(1) = 0.0;
00294 a(2) = ((R(3,3) == 1) ? 0.0 : M_PI);
00295 a(3) = atan2(R(2,1),R(1,1));
00296 } else {
00297 a(1) = atan2(R(1,3), -R(2,3));
00298 a(2) = atan2(sqrt(R(1,3)*R(1,3) + R(2,3)*R(2,3)), R(3,3));
00299 a(3) = atan2(R(3,1), R(3,2));
00300 }
00301
00302 a.Release(); return a;
00303 }
00304
00305 #ifdef use_namespace
00306 }
00307 #endif