00001
00002 #ifndef INCLUDED_fmatSpatial_h_
00003 #define INCLUDED_fmatSpatial_h_
00004
00005 #include "fmatCore.h"
00006
00007 namespace fmat {
00008
00009 template<typename R> class TransformT;
00010
00011
00012
00013
00014 template<typename R>
00015 inline Matrix<2,2,R> rotation2DT(R rad) {
00016 R s = std::sin(rad), c = std::cos(rad);
00017 R dat[4] = { c, s, -s, c };
00018 return Matrix<2,2,R>(dat);
00019 }
00020
00021 inline Matrix<2,2,fmatReal> rotation2D(fmatReal rad) { return rotation2DT<fmatReal>(rad); }
00022
00023
00024 template<size_t N, typename R>
00025 inline Matrix<N,N,R> rotationXN(R rad) {
00026 (void)sizeof(fmat_internal::CompileTimeAssert<Rotation_requires_larger_dimensions,N>=3>);
00027 Matrix<N,N,R> ans = Matrix<N,N,R>::identity();
00028 ans(1,1) = ans(2,2) = std::cos(rad);
00029 ans(1,2) = -(ans(2,1) = std::sin(rad));
00030 return ans;
00031 }
00032
00033 inline Matrix<3,3,fmatReal> rotationX(fmatReal rad) { return rotationXN<3,fmatReal>(rad); }
00034
00035
00036 template<size_t N, typename R>
00037 inline Matrix<N,N,R> rotationYN(R rad) {
00038 (void)sizeof(fmat_internal::CompileTimeAssert<Rotation_requires_larger_dimensions,N>=3>);
00039 Matrix<N,N,R> ans = Matrix<N,N,R>::identity();
00040 ans(0,0) = ans(2,2) = std::cos(rad);
00041 ans(2,0) = -(ans(0,2) = std::sin(rad));
00042 return ans;
00043 }
00044
00045 inline Matrix<3,3,fmatReal> rotationY(fmatReal rad) { return rotationYN<3,fmatReal>(rad); }
00046
00047
00048 template<size_t N, typename R>
00049 inline Matrix<N,N,R> rotationZN(R rad) {
00050 (void)sizeof(fmat_internal::CompileTimeAssert<Rotation_requires_larger_dimensions,N>=2>);
00051 if(N==2) {
00052 R s = std::sin(rad), c = std::cos(rad);
00053 R dat[4] = { c, s, -s, c };
00054 return Matrix<N,N,R>(dat);
00055 } else {
00056 Matrix<N,N,R> ans = Matrix<N,N,R>::identity();
00057 ans(0,0) = ans(1,1) = std::cos(rad);
00058 ans(0,1) = -(ans(1,0) = std::sin(rad));
00059 return ans;
00060 }
00061 }
00062
00063 inline Matrix<3,3,fmatReal> rotationZ(fmatReal rad) { return rotationZN<3,fmatReal>(rad); }
00064
00065
00066
00067
00068
00069
00070 template<class R=fmatReal>
00071 class QuaternionT {
00072 public:
00073
00074 QuaternionT() : w(1), x(0), y(0), z(0) {}
00075
00076
00077 QuaternionT(R w_, R x_, R y_, R z_) : w(w_), x(x_), y(y_), z(z_) {}
00078
00079
00080 template<typename T> static QuaternionT from(const T& q) { return QuaternionT(q[0],q[1],q[2],q[3]); }
00081
00082 template<typename T> QuaternionT& importFrom(const T& q) { w=q[0]; x=q[1]; y=q[2]; z=q[3]; return *this; }
00083
00084 template<typename T> T exportTo() const { T q; q[0]=w; q[1]=x; q[2]=y; q[3]=z; return q; }
00085
00086 template<typename T> T& exportTo(T& q) const { q[0]=w; q[1]=x; q[2]=y; q[3]=z; return q; }
00087
00088 template<typename T> void exportTo(T& w_, T& x_, T& y_, T& z_) const { w_=w; x_=x; y_=y; z_=z; }
00089
00090 template<typename T> void exportTo(T& x_, T& y_, T& z_) const { if(w<0) { x_=-x; y_=-y; z_=-z; } else { x_=x; y_=y; z_=z; } }
00091
00092 R getW() const { return w; }
00093 R getX() const { return x; }
00094 R getY() const { return y; }
00095 R getZ() const { return z; }
00096
00097 bool operator==(const QuaternionT<R> &other) const { return w==other.w && x==other.x && y==other.y && z==other.z; }
00098
00099
00100 static const QuaternionT& identity() { return IDENTITY; }
00101 static const QuaternionT IDENTITY;
00102
00103
00104 static QuaternionT<R> aboutX(R rad) { return fmat::QuaternionT<R>(std::cos(rad/2),std::sin(rad/2),0,0); }
00105
00106 static QuaternionT<R> aboutY(R rad) { return fmat::QuaternionT<R>(std::cos(rad/2),0,std::sin(rad/2),0); }
00107
00108 static QuaternionT<R> aboutZ(R rad) { return fmat::QuaternionT<R>(std::cos(rad/2),0,0,std::sin(rad/2)); }
00109
00110
00111
00112 template<class T>
00113 static QuaternionT fromAxisAngle(const T& axis, R angle) {
00114 if(std::abs(angle) <= std::numeric_limits<R>::epsilon())
00115 return QuaternionT();
00116 R n = axis.sumSq();
00117 if(n <= std::numeric_limits<R>::epsilon())
00118 return QuaternionT();
00119 R s = std::sin(angle/2)/std::sqrt(n);
00120 return QuaternionT(std::cos(angle/2), axis[0]*s, axis[1]*s, axis[2]*s);
00121 }
00122
00123
00124
00125 template<class T>
00126 static QuaternionT fromAxis(const T& axis) {
00127 R n2 = static_cast<R>(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
00128 if(n2>1) {
00129 if(n2>1+std::numeric_limits<R>::epsilon()*10) {
00130 R n = std::sqrt(n2);
00131 std::cerr << "Warning: fmat::Quaternion axis constructor passed non-normalized |" << axis[0] << ',' << axis[1] << ',' << axis[2] << "| = " << n << " (err " << (n-1) << ')' << std::endl;
00132 R s = 1/n;
00133 return QuaternionT(0, axis[0]*s, axis[1]*s, axis[2]*s);
00134 } else {
00135
00136 R s = 1/n2;
00137 return QuaternionT(0, axis[0]*s, axis[1]*s, axis[2]*s);
00138 }
00139 } else {
00140 return QuaternionT(std::sqrt(1-n2),axis[0],axis[1],axis[2]);
00141 }
00142 }
00143
00144
00145
00146
00147
00148
00149
00150 template<class T>
00151 static QuaternionT fromMatrix(const T& rot) {
00152 QuaternionT quat = fmat_internal::NoInit();
00153 R tracem1 = rot(0,0) + rot(1,1) + rot(2,2);
00154 if( tracem1 >= 0 ) {
00155 const R r = std::sqrt(tracem1 + 1);
00156 quat.w = (R)0.5 * r;
00157 const R s = (R)0.5 / r;
00158 quat.x = ( rot(2,1) - rot(1,2) ) * s;
00159 quat.y = ( rot(0,2) - rot(2,0) ) * s;
00160 quat.z = ( rot(1,0) - rot(0,1) ) * s;
00161 } else {
00162 if ( rot(0,0) > rot(1,1) && rot(0,0) > rot(2,2) ) {
00163 const R r = std::sqrt( 1.0f + rot(0,0) - rot(1,1) - rot(2,2));
00164 quat.x = (R)0.5 * r;
00165 const R s = (R)0.5 / r;
00166 quat.w = (rot(2,1) - rot(1,2) ) * s;
00167 quat.y = (rot(0,1) + rot(1,0) ) * s;
00168 quat.z = (rot(0,2) + rot(2,0) ) * s;
00169 } else if (rot(1,1) > rot(2,2)) {
00170 const R r = std::sqrt( 1.0f + rot(1,1) - rot(0,0) - rot(2,2));
00171 quat.y = (R)0.5 * r;
00172 const R s = (R)0.5 / r;
00173 quat.w = (rot(0,2) - rot(2,0) ) * s;
00174 quat.x = (rot(0,1) + rot(1,0) ) * s;
00175 quat.z = (rot(1,2) + rot(2,1) ) * s;
00176 } else {
00177 const R r = std::sqrt( 1.0f + rot(2,2) - rot(0,0) - rot(1,1) );
00178 quat.z = (R)0.5 * r;
00179 const R s = (R)0.5 / r;
00180 quat.w = (rot(1,0) - rot(0,1) ) * s;
00181 quat.x = (rot(0,2) + rot(2,0) ) * s;
00182 quat.y = (rot(1,2) + rot(2,1) ) * s;
00183 }
00184 }
00185 return quat;
00186 }
00187
00188
00189
00190 Matrix<3,3,R> toMatrix() const {
00191 const R t2 = w*x;
00192 const R t3 = w*y;
00193 const R t4 = w*z;
00194 const R t5 = -x*x;
00195 const R t6 = x*y;
00196 const R t7 = x*z;
00197 const R t8 = -y*y;
00198 const R t9 = y*z;
00199 const R t10 = -z*z;
00200 const R values[9] = {
00201 2*(t8 + t10) + 1,
00202 2*(t4 + t6),
00203 2*(t7 - t3),
00204 2*(t6 - t4),
00205 2*(t5 + t10) + 1,
00206 2*(t2 + t9),
00207 2*(t3 + t7),
00208 2*(t9 - t2),
00209 2*(t5 + t8) + 1
00210 };
00211 return Matrix<3,3,R>(values);
00212 }
00213
00214 operator Matrix<3,3,R>() const { return toMatrix(); }
00215
00216
00217 Column<3,R> axis() const {
00218 R n2 = (x*x + y*y + z*z);
00219 if(n2 < std::numeric_limits<R>::epsilon())
00220 return fmat::packT<R>(0,0,1);
00221 R sc = 1/std::sqrt(n2);
00222 return fmat::packT<R>(x*sc, y*sc, z*sc);
00223 }
00224
00225
00226
00227 R angle() const {
00228 R n = std::sqrt(x*x + y*y + z*z);
00229 R ang = 2*std::atan2(n,w);
00230 if(ang>static_cast<R>(M_PI))
00231 ang-=2*static_cast<R>(M_PI);
00232 return ang;
00233
00234
00235
00236
00237 }
00238
00239
00240 template<class T>
00241 R axisComponent(const T& v) const {
00242
00243
00244 R n = std::sqrt(x*x + y*y + z*z);
00245 if(n < std::numeric_limits<R>::epsilon())
00246 return 0;
00247 R ang_2 = std::atan2(n,w);
00248 return 2*ang_2*(x*v[0] + y*v[1] + z*v[2])/n;
00249 }
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261 fmat::Column<3,R> ypr() const {
00262 const R sqw=w*w, sqx=x*x, sqy=y*y, sqz=z*z;
00263 const R unit = sqw + sqx + sqy + sqz;
00264 const R test = y*w - x*z;
00265 if (test > 0.4999*unit) {
00266 return packT<R>(-2 * std::atan2(x,w), (R)M_PI_2, 0);
00267 }
00268 if (test < -0.4999*unit) {
00269 return packT<R>(2 * std::atan2(x,w), -(R)M_PI_2, 0);
00270 }
00271 return packT<R>(
00272 std::atan2(2*z*w+2*x*y , sqx - sqz - sqy + sqw),
00273 std::asin(2*test/unit),
00274 std::atan2(2*x*w+2*z*y , -sqx + sqz - sqy + sqw)
00275 );
00276 }
00277
00278
00279 QuaternionT inverse() const ATTR_must_check { return QuaternionT(w,-x,-y,-z); }
00280
00281
00282
00283
00284
00285
00286
00287 template<typename Rb>
00288 QuaternionT<typename fmat_internal::promotion_trait<R,Rb>::type>
00289 operator*(const QuaternionT<Rb>& q) const {
00290 return QuaternionT<typename fmat_internal::promotion_trait<R,Rb>::type>(
00291 w*q.w - x*q.x - y*q.y - z*q.z,
00292 w*q.x + x*q.w + y*q.z - z*q.y,
00293 w*q.y - x*q.z + y*q.w + z*q.x,
00294 w*q.z + x*q.y - y*q.x + z*q.w
00295 );
00296 }
00297
00298
00299 QuaternionT operator*=(const QuaternionT& q) { return this->operator=(operator*(q)); }
00300
00301
00302 template<template<size_t H, size_t W, typename Rt> class T, size_t W>
00303 Matrix<3,W,R> operator*(const T<3,W,R>& m) const {
00304 const R t2 = w*x;
00305 const R t3 = w*y;
00306 const R t4 = w*z;
00307 const R t5 = -x*x;
00308 const R t6 = x*y;
00309 const R t7 = x*z;
00310 const R t8 = -y*y;
00311 const R t9 = y*z;
00312 const R t10 = -z*z;
00313 Matrix<3,W,R> o=fmat_internal::NoInit();
00314 for(size_t c = 0; c<W; ++c) {
00315 o(0,c) = 2*( (t8 + t10)*m(0,c) + (t6 - t4)*m(1,c) + (t3 + t7)*m(2,c) ) + m(0,c);
00316 o(1,c) = 2*( (t4 + t6)*m(0,c) + (t5 + t10)*m(1,c) + (t9 - t2)*m(2,c) ) + m(1,c);
00317 o(2,c) = 2*( (t7 - t3)*m(0,c) + (t2 + t9)*m(1,c) + (t5 + t8)*m(2,c) ) + m(2,c);
00318 }
00319 return o;
00320 }
00321
00322
00323 TransformT<R> operator*(const TransformT<R>& m) const {
00324 const R t2 = w*x;
00325 const R t3 = w*y;
00326 const R t4 = w*z;
00327 const R t5 = -x*x;
00328 const R t6 = x*y;
00329 const R t7 = x*z;
00330 const R t8 = -y*y;
00331 const R t9 = y*z;
00332 const R t10 = -z*z;
00333 TransformT<R> o=fmat_internal::NoInit();
00334 for(size_t c = 0; c<4; ++c) {
00335 o(0,c) = 2*( (t8 + t10)*m(0,c) + (t6 - t4)*m(1,c) + (t3 + t7)*m(2,c) ) + m(0,c);
00336 o(1,c) = 2*( (t4 + t6)*m(0,c) + (t5 + t10)*m(1,c) + (t9 - t2)*m(2,c) ) + m(1,c);
00337 o(2,c) = 2*( (t7 - t3)*m(0,c) + (t2 + t9)*m(1,c) + (t5 + t8)*m(2,c) ) + m(2,c);
00338 }
00339 return o;
00340 }
00341
00342
00343 Column<3,R> operator*(const Column<3,R> v) const {
00344 const R t2 = w*x;
00345 const R t3 = w*y;
00346 const R t4 = w*z;
00347 const R t5 = -x*x;
00348 const R t6 = x*y;
00349 const R t7 = x*z;
00350 const R t8 = -y*y;
00351 const R t9 = y*z;
00352 const R t10 = -z*z;
00353 Column<3,R> o=fmat_internal::NoInit();
00354 o[0] = 2*( (t8 + t10)*v[0] + (t6 - t4)*v[1] + (t3 + t7)*v[2] ) + v[0];
00355 o[1] = 2*( (t4 + t6)*v[0] + (t5 + t10)*v[1] + (t9 - t2)*v[2] ) + v[1];
00356 o[2] = 2*( (t7 - t3)*v[0] + (t2 + t9)*v[1] + (t5 + t8)*v[2] ) + v[2];
00357 return o;
00358 }
00359
00360
00361 R sumSq() const { return w*w + x*x + y*y + z*z; }
00362
00363
00364 R norm() const { return std::sqrt(w*w + x*x + y*y + z*z); }
00365
00366
00367 R normalize() {
00368 R n = sumSq();
00369 if(n < std::numeric_limits<R>::epsilon()) {
00370 w=x=y=z=0;
00371 return 0;
00372 }
00373 if(n<=1 && 1-n < std::numeric_limits<R>::epsilon()) {
00374
00375 if(w<0) {
00376 w=-w;
00377 x=-x;
00378 y=-y;
00379 z=-z;
00380 }
00381 return 1;
00382 }
00383 R sc = (w<0?-1:1) / std::sqrt(n);
00384 w*=sc;
00385 x*=sc;
00386 y*=sc;
00387 z*=sc;
00388 return n;
00389 }
00390
00391 friend inline std::ostream& operator<<(std::ostream& os, const QuaternionT<R>& q) { return os << fmat::SubVector<4,const R>((const R*)&q.w); }
00392
00393 protected:
00394 R w, x, y, z;
00395
00396
00397 QuaternionT(const fmat_internal::NoInit&) : w(), x(), y(), z() {}
00398 };
00399 typedef QuaternionT<> Quaternion;
00400
00401
00402 template<class Ra, class Rb>
00403 QuaternionT<typename fmat_internal::promotion_trait<Ra,Rb>::type>
00404 crossProduct(const QuaternionT<Ra>& p, const QuaternionT<Rb>& q) { return q * p.inverse(); }
00405
00406
00407 template<typename R> QuaternionT<R> invert(const QuaternionT<R>& q) { return q.inverse(); }
00408
00409
00410
00411
00412
00413 namespace fmat_internal {
00414
00415 template<typename R1, typename R2>
00416 struct transformOps {
00417
00418 typedef typename fmat_internal::promotion_trait<R1,R2>::type out_t;
00419
00420 template<typename V>
00421 inline static Column<3,out_t> multiply3(const TransformT<R1>& tr, const V& x) {
00422 Column<3,out_t> ans=fmat_internal::NoInit();
00423 const R1 *a = &tr(0,0);
00424 const R2 *b = &x[0];
00425 ans[0] = a[0+3*0]*b[0] + a[0+3*1]*b[1] + a[0+3*2]*b[2] + a[0+3*3];
00426 ans[1] = a[1+3*0]*b[0] + a[1+3*1]*b[1] + a[1+3*2]*b[2] + a[1+3*3];
00427 ans[2] = a[2+3*0]*b[0] + a[2+3*1]*b[1] + a[2+3*2]*b[2] + a[2+3*3];
00428 return ans;
00429 }
00430
00431 template<typename V>
00432 inline static Column<4,out_t> multiply4(const TransformT<R1>& tr, const V& x) {
00433 Column<4,out_t> ans=fmat_internal::NoInit();
00434 const R1 *a = &tr(0,0);
00435 const R2 *b = &x[0];
00436 ans[0] = a[0+3*0]*b[0] + a[0+3*1]*b[1] + a[0+3*2]*b[2] + a[0+3*3]*b[3];
00437 ans[1] = a[1+3*0]*b[0] + a[1+3*1]*b[1] + a[1+3*2]*b[2] + a[1+3*3]*b[3];
00438 ans[2] = a[2+3*0]*b[0] + a[2+3*1]*b[1] + a[2+3*2]*b[2] + a[2+3*3]*b[3];
00439 ans[3] = b[3];
00440 return ans;
00441 }
00442
00443 inline static void multiplyT(const R1* a, const R2* b, out_t* ans) {
00444 ans[0+3*0] = a[0+3*0]*b[0+3*0] + a[0+3*1]*b[1+3*0] + a[0+3*2]*b[2+3*0];
00445 ans[1+3*0] = a[1+3*0]*b[0+3*0] + a[1+3*1]*b[1+3*0] + a[1+3*2]*b[2+3*0];
00446 ans[2+3*0] = a[2+3*0]*b[0+3*0] + a[2+3*1]*b[1+3*0] + a[2+3*2]*b[2+3*0];
00447 ans[0+3*1] = a[0+3*0]*b[0+3*1] + a[0+3*1]*b[1+3*1] + a[0+3*2]*b[2+3*1];
00448 ans[1+3*1] = a[1+3*0]*b[0+3*1] + a[1+3*1]*b[1+3*1] + a[1+3*2]*b[2+3*1];
00449 ans[2+3*1] = a[2+3*0]*b[0+3*1] + a[2+3*1]*b[1+3*1] + a[2+3*2]*b[2+3*1];
00450 ans[0+3*2] = a[0+3*0]*b[0+3*2] + a[0+3*1]*b[1+3*2] + a[0+3*2]*b[2+3*2];
00451 ans[1+3*2] = a[1+3*0]*b[0+3*2] + a[1+3*1]*b[1+3*2] + a[1+3*2]*b[2+3*2];
00452 ans[2+3*2] = a[2+3*0]*b[0+3*2] + a[2+3*1]*b[1+3*2] + a[2+3*2]*b[2+3*2];
00453 ans[0+3*3] = a[0+3*0]*b[0+3*3] + a[0+3*1]*b[1+3*3] + a[0+3*2]*b[2+3*3] + a[0+3*3];
00454 ans[1+3*3] = a[1+3*0]*b[0+3*3] + a[1+3*1]*b[1+3*3] + a[1+3*2]*b[2+3*3] + a[1+3*3];
00455 ans[2+3*3] = a[2+3*0]*b[0+3*3] + a[2+3*1]*b[1+3*3] + a[2+3*2]*b[2+3*3] + a[2+3*3];
00456 }
00457 };
00458 }
00459
00460
00461
00462 template<typename R=fmatReal>
00463 class TransformT : public Matrix<3,4,R> {
00464 public:
00465 enum {
00466 HEIGHT=3, H=3,
00467 WIDTH=4, W=4,
00468 CAP=12
00469 };
00470 typedef R storage_t;
00471 typedef typename fmat_internal::unconst<R>::type out_t;
00472
00473 TransformT(const fmat_internal::NoInit& noinit) : Matrix<H,W,R>(noinit) {}
00474
00475 TransformT() : Matrix<H,W,R>(Matrix<H,W,R>::identity()) { }
00476 explicit TransformT(const R* x, size_t colStride=H) : Matrix<H,W,R>(x,colStride) {}
00477 TransformT(const SubMatrix<H,W,R>& x) : Matrix<H,W,R>(x) {}
00478 TransformT(const Matrix<H,W,R>& x) : Matrix<H,W,R>(x) {}
00479 template<typename Rot, typename Pt> TransformT(const Rot& r, const Pt& p) : Matrix<H,W,R>(fmat_internal::NoInit()) { rotation()=r; translation()=p; }
00480 template<size_t SH, size_t SW> explicit TransformT(const SubMatrix<SH,SW,R>& x) : Matrix<H,W,R>(x) {}
00481 template<size_t SH, size_t SW> explicit TransformT(const SubMatrix<SH,SW,R>& x, size_t rowOff, size_t colOff) : Matrix<H,W,R>(x,rowOff,colOff) {}
00482 template<size_t SH, size_t SW> explicit TransformT(const Matrix<SH,SW,R>& x) : Matrix<H,W,R>(x) {}
00483 template<size_t SH, size_t SW> explicit TransformT(const Matrix<SH,SW,R>& x, size_t rowOff, size_t colOff) : Matrix<H,W,R>(x,rowOff,colOff) {}
00484
00485 SubMatrix<3,3,R> rotation() { return SubMatrix<3,3,R>(data); }
00486 SubMatrix<3,3,const R> rotation() const { return SubMatrix<3,3,const R>(data); }
00487 SubVector<3,R> translation() { return Matrix<H,W,R>::column(3); }
00488 SubVector<3,const R> translation() const { return Matrix<H,W,R>::column(3); }
00489
00490
00491 static TransformT<R> aboutX(R rad) {
00492 TransformT<R> ans;
00493 ans(1,1) = ans(2,2) = std::cos(rad);
00494 ans(1,2) = -(ans(2,1) = std::sin(rad));
00495 return ans;
00496 }
00497
00498 static TransformT<R> aboutY(R rad) {
00499 TransformT<R> ans;
00500 ans(0,0) = ans(2,2) = std::cos(rad);
00501 ans(2,0) = -(ans(0,2) = std::sin(rad));
00502 return ans;
00503 }
00504
00505 static TransformT<R> aboutZ(R rad) {
00506 TransformT<R> ans;
00507 ans(0,0) = ans(1,1) = std::cos(rad);
00508 ans(0,1) = -(ans(1,0) = std::sin(rad));
00509 return ans;
00510 }
00511
00512 template<class V>
00513 static TransformT<R> offset(const V& x) { TransformT<R> ans; ans.translation() = x; return ans; }
00514
00515 static const TransformT& identity() { return IDENTITY; }
00516 static const TransformT IDENTITY;
00517
00518
00519 inline TransformT inverse() const ATTR_must_check {
00520 TransformT ans=fmat_internal::NoInit();
00521 ans.rotation() = fmat::invert(rotation());
00522 ans.translation() = -ans.rotation() * translation();
00523 return ans;
00524 }
00525
00526
00527 inline TransformT rigidInverse() const ATTR_must_check {
00528 TransformT ans=fmat_internal::NoInit();
00529 R * t2dat = &ans(0,0);
00530 t2dat[0] = data[0];
00531 t2dat[1] = data[3];
00532 t2dat[2] = data[6];
00533 t2dat[3] = data[1];
00534 t2dat[4] = data[4];
00535 t2dat[5] = data[7];
00536 t2dat[6] = data[2];
00537 t2dat[7] = data[5];
00538 t2dat[8] = data[8];
00539 t2dat[9] = -(data[0]*data[9] + data[1]*data[10] + data[2]*data[11]);
00540 t2dat[10] = -(data[3]*data[9] + data[4]*data[10] + data[5]*data[11]);
00541 t2dat[11] = -(data[6]*data[9] + data[7]*data[10] + data[8]*data[11]);
00542 return ans;
00543 }
00544
00545 template<typename R2>
00546 inline Column<3,typename fmat_internal::promotion_trait<R,R2>::type>
00547 operator*(const Column<3,R2>& x) const {
00548 return fmat_internal::transformOps<R,R2>::multiply3(*this,x);
00549 }
00550
00551 template<typename R2>
00552 inline Column<3,typename fmat_internal::promotion_trait<R,R2>::type>
00553 operator*(const SubVector<3,R2>& x) const {
00554 return fmat_internal::transformOps<R,R2>::multiply3(*this,x);
00555 }
00556
00557 template<size_t N, typename R2>
00558 inline Matrix<3,N,typename fmat_internal::promotion_trait<R,R2>::type>
00559 operator*(const Matrix<3,N,R2>& x) const {
00560 Matrix<3,N,typename fmat_internal::promotion_trait<R,R2>::type> ans = fmat_internal::NoInit();
00561 for(size_t i=0; i<N; ++i)
00562 ans.column(i) = fmat_internal::transformOps<R,R2>::multiply3(*this,x.column(i));
00563 return ans;
00564 }
00565
00566 template<typename R2>
00567 inline Column<4,typename fmat_internal::promotion_trait<R,R2>::type>
00568 operator*(const Column<4,R2>& x) const {
00569 return fmat_internal::transformOps<R,R2>::multiply4(*this,x);
00570 }
00571
00572 template<typename R2>
00573 inline Column<4,typename fmat_internal::promotion_trait<R,R2>::type>
00574 operator*(const SubVector<4,R2>& x) const {
00575 return fmat_internal::transformOps<R,R2>::multiply4(*this,x);
00576 }
00577
00578 template<size_t N, typename R2>
00579 inline Matrix<4,N,typename fmat_internal::promotion_trait<R,R2>::type>
00580 operator*(const Matrix<4,N,R2>& x) const {
00581 Matrix<4,N,typename fmat_internal::promotion_trait<R,R2>::type> ans = fmat_internal::NoInit();
00582 for(size_t i=0; i<N; ++i)
00583 ans.column(i) = fmat_internal::transformOps<R,R2>::multiply4(*this,x.column(i));
00584 return ans;
00585 }
00586
00587 template<typename R2>
00588 inline TransformT<typename fmat_internal::promotion_trait<R,R2>::type>
00589 operator*(const TransformT<R2>& tb) const {
00590 TransformT<typename fmat_internal::promotion_trait<R,R2>::type> mans=fmat_internal::NoInit();
00591 fmat_internal::transformOps<R,R2>::multiplyT(data,&tb(0,0),&mans(0,0));
00592 return mans;
00593 }
00594
00595 template<typename R2>
00596 TransformT&
00597 operator*=(const TransformT<R2>& t) {
00598 fmat_internal::transformOps<R,R2>::multiplyT(&TransformT(*this)(0,0),&t(0,0),data);
00599 return *this;
00600 }
00601 using Matrix<H,W,R>::operator=;
00602 using Matrix<H,W,R>::operator*=;
00603 using Matrix<H,W,R>::operator/=;
00604
00605 protected:
00606 using Matrix<H,W,R>::data;
00607 };
00608 typedef TransformT<> Transform;
00609
00610
00611 template<typename R>
00612 inline Matrix<4,4,R> invertTransform(const Matrix<4,4,R>& t) {
00613 fmat::Column<4> p = -t.column(3);
00614 const R * tdat = &t(0,0);
00615
00616
00617
00618
00619
00620
00621
00622 Matrix<4,4,R> t2(t);
00623 R * t2dat = &t2(0,0);
00624 t2dat[1] = tdat[4];
00625 t2dat[2] = tdat[8];
00626 t2dat[4] = tdat[1];
00627 t2dat[6] = tdat[9];
00628 t2dat[8] = tdat[2];
00629 t2dat[9] = tdat[6];
00630 t2dat[12] = fmat::dotProduct(t.column(0),p);
00631 t2dat[13] = fmat::dotProduct(t.column(1),p);
00632 t2dat[14] = fmat::dotProduct(t.column(2),p);
00633 return t2;
00634 }
00635
00636
00637 template<typename R>
00638 TransformT<R>
00639 invert(const TransformT<R>& m) { return m.inverse(); }
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651 template<typename R>
00652 Column<3,R>
00653 ypr(const QuaternionT<R>& q) { return q.ypr(); }
00654
00655
00656
00657
00658
00659
00660
00661
00662 template<typename R>
00663 Column<3,R>
00664 ypr(const TransformT<R>& m) { return ypr(m.rotation()); }
00665
00666
00667
00668
00669
00670
00671
00672
00673 template<template<size_t,size_t,typename R> class M, typename R>
00674 Column<3,typename fmat_internal::unconst<R>::type>
00675 ypr(const M<3,3,R>& m) {
00676 typedef typename fmat_internal::unconst<R>::type out_t;
00677 if (m(2,0) > 0.9998) {
00678 return packT<out_t>(std::atan2(-m(0,1),m(1,1)), -M_PI_2, 0);
00679 }
00680 if (m(2,0) < -0.9998) {
00681 return packT<out_t>(std::atan2(-m(0,1),m(1,1)), M_PI_2, 0);
00682 }
00683 return packT<out_t>(
00684 std::atan2(m(1,0),m(0,0)),
00685 std::asin(-m(2,0)),
00686 std::atan2(m(2,1),m(2,2))
00687 );
00688 }
00689
00690
00691
00692 template<typename R>
00693 Column<2,R>
00694 segmentIntersection(const Column<2,R>& p1, const Column<2,R>& d1, const Column<2,R>& p2, const Column<2,R>& d2) {
00695 fmat::Matrix<2,2,R> dm = fmat_internal::NoInit();
00696 dm.column(0) = d1;
00697 dm.column(1) = -d2;
00698 try {
00699 dm = invert(dm);
00700 } catch(...) {
00701
00702 return Column<2,R>(std::numeric_limits<fmat::fmatReal>::infinity());
00703 }
00704 Column<2,R> o = p2-p1;
00705 return dm * o;
00706 }
00707
00708
00709
00710 template<typename R>
00711 R
00712 rayIntersection(const Column<2,R>& p1, const Column<2,R>& d1, const Column<2,R>& p2, const Column<2,R>& d2) {
00713 return segmentIntersection(p1, d1, p2, d2)[0];
00714 }
00715
00716
00717
00718
00719 template<typename R>
00720 Column<2,R>
00721 lineIntersection(const Column<2,R>& p1, const Column<2,R>& d1, const Column<2,R>& p2, const Column<2,R>& d2) {
00722 return d1 * rayIntersection(p1,d1,p2,d2) + p1;
00723 }
00724
00725
00726 template<template<size_t,typename R> class V, typename R>
00727 Column<2,typename V<2,R>::out_t>
00728 normalLeft(const V<2,R>& v) { return fmat::pack(-v[1],v[0]); }
00729
00730
00731 template<template<size_t,typename R> class V, typename R>
00732 Column<2,typename V<2,R>::out_t>
00733 normalRight(const V<2,R>& v) { return fmat::pack(v[1],-v[0]); }
00734 }
00735
00736
00737
00738
00739
00740
00741 #endif