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