75 inline T r()
const {
return (*
this)[0];}
76 inline T x()
const {
return (*
this)[1];}
77 inline T y()
const {
return (*
this)[2];}
78 inline T z()
const {
return (*
this)[3];}
79 inline void r(
const T r) {(*this)[0]=r;}
80 inline void x(
const T x) {(*this)[1]=x;}
81 inline void y(
const T y) {(*this)[2]=y;}
82 inline void z(
const T z) {(*this)[3]=z;}
95 template <
class ARRAYLIKE3>
103 const T angle = std::sqrt(x*x+y*y+z*z);
107 (*this)[1] =
static_cast<T
>(0.5)*x;
108 (*this)[2] =
static_cast<T
>(0.5)*y;
109 (*this)[3] =
static_cast<T
>(0.5)*z;
113 const T s = (::sin(angle/2))/angle;
114 const T c = ::cos(angle/2);
131 template <
class ARRAYLIKE3>
132 inline void ln(ARRAYLIKE3 &out_ln)
const
134 if (out_ln.size()!=3) out_ln.resize(3);
138 template <
class ARRAYLIKE3>
139 inline ARRAYLIKE3
ln()
const
146 template <
class ARRAYLIKE3>
151 const T K = (xyz_norm<1e-7) ? 2 : 2*::acos(r())/xyz_norm;
159 template <
class ARRAYLIKE3>
167 template <
class ARRAYLIKE3>
182 const T new_r = q1.r()*q2.r() - q1.x()*q2.x() - q1.y()*q2.y() - q1.z()*q2.z();
183 const T new_x = q1.r()*q2.x() + q2.r()*q1.x() + q1.y()*q2.z() - q2.y()*q1.z();
184 const T new_y = q1.r()*q2.y() + q2.r()*q1.y() + q1.z()*q2.x() - q2.z()*q1.x();
185 const T new_z = q1.r()*q2.z() + q2.r()*q1.z() + q1.x()*q2.y() - q2.x()*q1.y();
186 this->r(new_r); this->x(new_x); this->y(new_y); this->z(new_z);
192 void rotatePoint(
const double lx,
const double ly,
const double lz,
double &gx,
double &gy,
double &gz )
const
194 const double t2 = r()*x();
const double t3 = r()*y();
const double t4 = r()*z();
const double t5 =-x()*x();
const double t6 = x()*y();
195 const double t7 = x()*z();
const double t8 =-y()*y();
const double t9 = y()*z();
const double t10=-z()*z();
196 gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
197 gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
198 gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
203 void inverseRotatePoint(
const double lx,
const double ly,
const double lz,
double &gx,
double &gy,
double &gz )
const
205 const double t2 =-r()*x();
const double t3 =-r()*y();
const double t4 =-r()*z();
const double t5 =-x()*x();
const double t6 = x()*y();
206 const double t7 = x()*z();
const double t8 =-y()*y();
const double t9 = y()*z();
const double t10=-z()*z();
207 gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
208 gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
209 gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
222 const T qq = 1.0/std::sqrt(
normSqr() );
223 for (
unsigned int i=0;i<4;i++)
230 template <
class MATRIXLIKE>
233 const T n = 1.0/std::pow(
normSqr(),T(1.5));
235 J.get_unsafe(0,0)=x()*x()+y()*y()+z()*z();
236 J.get_unsafe(0,1)=-r()*x();
237 J.get_unsafe(0,2)=-r()*y();
238 J.get_unsafe(0,3)=-r()*z();
240 J.get_unsafe(1,0)=-x()*r();
241 J.get_unsafe(1,1)=r()*r()+y()*y()+z()*z();
242 J.get_unsafe(1,2)=-x()*y();
243 J.get_unsafe(1,3)=-x()*z();
245 J.get_unsafe(2,0)=-y()*r();
246 J.get_unsafe(2,1)=-y()*x();
247 J.get_unsafe(2,2)=r()*r()+x()*x()+z()*z();
248 J.get_unsafe(2,3)=-y()*z();
250 J.get_unsafe(3,0)=-z()*r();
251 J.get_unsafe(3,1)=-z()*x();
252 J.get_unsafe(3,2)=-z()*y();
253 J.get_unsafe(3,3)=r()*r()+x()*x()+y()*y();
260 template <
class MATRIXLIKE>
264 J.get_unsafe(0,0)=r(); J.get_unsafe(0,1)=-x(); J.get_unsafe(0,2)=-y(); J.get_unsafe(0,3)=-z();
265 J.get_unsafe(1,0)=x(); J.get_unsafe(1,1)= r(); J.get_unsafe(1,2)=-z(); J.get_unsafe(1,3)= y();
266 J.get_unsafe(2,0)=y(); J.get_unsafe(2,1)= z(); J.get_unsafe(2,2)= r(); J.get_unsafe(2,3)=-x();
267 J.get_unsafe(3,0)=z(); J.get_unsafe(3,1)=-y(); J.get_unsafe(3,2)= x(); J.get_unsafe(3,3)= r();
279 template <
class MATRIXLIKE>
287 template <
class MATRIXLIKE>
290 M.get_unsafe(0,0)=r()*r()+x()*x()-y()*y()-z()*z(); M.get_unsafe(0,1)=2*(x()*y() -r()*z()); M.get_unsafe(0,2)=2*(z()*x()+r()*y());
291 M.get_unsafe(1,0)=2*(x()*y()+r()*z()); M.get_unsafe(1,1)=r()*r()-x()*x()+y()*y()-z()*z(); M.get_unsafe(1,2)=2*(y()*z()-r()*x());
292 M.get_unsafe(2,0)=2*(z()*x()-r()*y()); M.get_unsafe(2,1)=2*(y()*z()+r()*x()); M.get_unsafe(2,2)=r()*r()-x()*x()-y()*y()+z()*z();
327 template <
class MATRIXLIKE>
333 if (out_dr_dq && resize_out_dr_dq_to3x4)
334 out_dr_dq->setSize(3,4);
335 const T discr = r()*y()-x()*z();
336 if (fabs(discr)>0.49999)
339 yaw =-2*atan2(x(),r());
343 out_dr_dq->get_unsafe(0,0) = +2/x();
344 out_dr_dq->get_unsafe(0,2) = -2*r()/(x()*x());
347 else if (discr<-0.49999)
350 yaw = 2*atan2(x(),r());
354 out_dr_dq->get_unsafe(0,0) = -2/x();
355 out_dr_dq->get_unsafe(0,2) = +2*r()/(x()*x());
360 yaw = ::atan2( 2*(r()*z()+x()*y()), 1-2*(y()*y()+z()*z()) );
361 pitch = ::asin ( 2*discr );
362 roll = ::atan2( 2*(r()*x()+y()*z()), 1-2*(x()*x()+y()*y()) );
365 const double val1=(2*x()*x() + 2*y()*y() - 1);
366 const double val12=
square(val1);
367 const double val2=(2*r()*x() + 2*y()*z());
368 const double val22=
square(val2);
369 const double xy2 = 2*x()*y();
370 const double rz2 = 2*r()*z();
371 const double ry2 = 2*r()*y();
372 const double val3 = (2*y()*y() + 2*z()*z() - 1);
373 const double val4 = ((
square(rz2 + xy2)/
square(val3) + 1)*val3);
374 const double val5 = (4*(rz2 + xy2))/
square(val3);
375 const double val6 = 1.0/(
square(rz2 + xy2)/
square(val3) + 1);
376 const double val7 = 2.0/ sqrt(1 -
square(ry2 - 2*x()*z()));
377 const double val8 = (val22/val12 + 1);
378 const double val9 = -2.0/val8;
380 out_dr_dq->get_unsafe(0,0) = -2*z()/val4;
381 out_dr_dq->get_unsafe(0,1) = -2*y()/val4;
382 out_dr_dq->get_unsafe(0,2) = -(2*x()/val3 - y()*val5)*val6 ;
383 out_dr_dq->get_unsafe(0,3) = -(2*r()/val3 - z()*val5)*val6;
385 out_dr_dq->get_unsafe(1,0) = y()*val7 ;
386 out_dr_dq->get_unsafe(1,1) = -z()*val7 ;
387 out_dr_dq->get_unsafe(1,2) = r()*val7 ;
388 out_dr_dq->get_unsafe(1,3) = -x()*val7 ;
390 out_dr_dq->get_unsafe(2,0) = val9*x()/val1 ;
391 out_dr_dq->get_unsafe(2,1) = val9*(r()/val1 - (2*x()*val2)/val12) ;
392 out_dr_dq->get_unsafe(2,2) = val9*(z()/val1 - (2*y()*val2)/val12) ;
393 out_dr_dq->get_unsafe(2,3) = val9*y()/val1 ;