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