开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
Xform.h
资源名称：scanalyze-1.0.3_source_code.rar　[点击查看]
上传用户：kellyonhid
上传日期：2013-10-12
资源大小：932k
文件大小：37k
源码类别：

3D图形编程

开发平台：

Visual C++

Xform.h：源码内容
  1. ////////////////////////////////////////////////////////////
  2. // Xform.h
  3. //
  4. // A class for doing 3D transformations using 4x4 matrices
  5. ////////////////////////////////////////////////////////////
  7. #ifndef _xform_h_
  8. #define _xform_h_
  10. #include <assert.h>
  11. #include <math.h>
  12. #include <float.h>
  13. #include <iostream.h>
  14. #include <iomanip.h>
  15. #include <algo.h>
  16. #include "Pnt3.h"
  19. #ifdef __GNUC__
  20. // G++ implements the standard for templated friend functions of template
  21. // classes.  This means that the following forward declarations are necessary..
  23. template <class T>
  24. class Xform;
  26. template <class T>
  27. Xform<T> operator*(const Xform<T> &a, const Xform<T> &b); 
  29. template <class T>
  30. Xform<T> delta(Xform<T> a, const Xform<T> &b); 
  32. template <class T>
  33. Xform<T> linear_interp(const Xform<T> &a,
  34.                        const Xform<T> &b,
  35.                        double          t);
  37. template <class T>
  38. Xform<T> bilinear_interp(const Xform<T> &u0v0,
  39.                          const Xform<T> &u1v0,
  40.                          const Xform<T> &u0v1,
  41.                          const Xform<T> &u1v1,
  42.                          double u, double v);
  44. template <class T>
  45. Xform<T> relative_xform(const Xform<T> &a, Xform<T> b);
  47. template <class T>
  48. ostream& operator<<(ostream &out, const Xform<T> &xf);
  50. template <class T>
  51. istream& operator>>(istream & in, Xform<T> &xf);
  53. #endif
  56. // logically Xform is applied to a point: p' = M*p
  57. // however, the transpose of the M is stored 
  58. // to keep it compatible with OpenGL
  60. template <class T> // use only with float or double
  61. class Xform {
  62. private:
  63.   T m[4][4];         // column major order
  65. public:
  66.   Xform(void);
  67.   Xform(const double *a);  // from OpenGL matrix
  68.   Xform(const float  *a);  // from OpenGL matrix
  69.   Xform(const T r[3][3], const T t[3]);
  70.   Xform<T> &operator=(const double *a);
  71.   Xform<T> &operator=(const float *a);
  72.   Xform<T> &operator=(const Xform<T> &xf);
  74.   Xform<T> &fromQuaternion(const double q[4], // q == {w,x,y,z}
  75.    double tx = 0.0,
  76.    double ty = 0.0, 
  77.    double tz = 0.0);
  78.   Xform<T> &fromQuaternion(const float q[4],  // q == {w,x,y,z}
  79.    float tx = 0.0,
  80.    float ty = 0.0, 
  81.    float tz = 0.0);
  82.   void toQuaternion(double q[4]) const;
  83.   void toQuaternion(float q[4]) const;
  84.   Xform<T> &addQuaternion(T q0, T q1, T q2, T q3, 
  85.   T tx = 0.0, T ty = 0.0, T tz = 0.0);
  86.   Xform<T> &addQuaternion(const float  *q, int n = 4);  // n == 4,7
  87.   Xform<T> &addQuaternion(const double *q, int n = 4);  // n == 4,7
  89.   void fromEuler(const T ea[3], int order);
  90.   void toEuler(T ea[3], int order);
  92.   Xform<T> &fromFrame(const Pnt3 &x, 
  93.       const Pnt3 &y,
  94.       const Pnt3 &z,
  95.       const Pnt3 &org);
  96.   Xform<T> &fromRotTrans(const T r[3][3], const T t[3]);
  98.   bool isIdentity (void) const;
  99.   Xform<T> &identity(void);             // set to identity
  100.   Xform<T> &fast_invert(void);          // assume rot + trans
  101.   Xform<T> &invert(void);               // general inversion
  103.   void translate(const double t[3]);    // M <- T*M
  104.   void translate(const float  t[3]);    // M <- T*M
  105.   void translate(T x, T y, T z);        // M <- T*M
  106.   void getTranslation(double t[3]) const;
  107.   void getTranslation(float  t[3]) const;
  109.   void removeTranslation(void);
  110.   void removeTranslation(double t[3]);
  111.   void removeTranslation(float t[3]);
  113.   void scale(const double t[3]);    // M <- S*M
  114.   void scale(const float  t[3]);    // M <- S*M
  115.   void scale(T x, T y, T z);        // M <- S*M
  117.   void get_e(int i, T e[3]) const; // get a basis vector, i=0,1,2
  119.   Xform<T> &rotX(T a);                  // M <- R*M
  120.   Xform<T> &rotY(T a);                  // M <- R*M
  121.   Xform<T> &rotZ(T a);                  // M <- R*M
  123.   Xform<T> &rot(T angle, T ax, T ay, T az); // M <- R*M
  124.   Xform<T> &rot(T angle, T ax, T ay, T az,
  125. T ox, T oy, T oz); // M <- R*M
  126.   Xform<T> &rotQ(T q0, T q1, T q2, T q3);   // M <- R*M
  127.   void      get_rot(T &angle, T &ax, T &ay, T &az);
  129.   void apply(const double i[3], double o[3]) const; // o = M*i
  130.   void apply(const float i[3],  float o[3]) const;  // o = M*i
  131.   void apply_inv(const double i[3], double o[3]) const; // o = M^-1*i
  132.   void apply_inv(const float i[3],  float o[3]) const;  // o = M^-1*i
  133.   void operator()(double i[3]) const;   // i <- M*i 
  134.   void operator()(float  i[3]) const;   // i <- M*i 
  136.   // apply to normal vector (no translation, only rotation)
  137.   void apply_nrm(const double i[3], double o[3]) const; // o = M*i
  138.   void apply_nrm(const float i[3],  float o[3]) const;  // o = M*i
  140.   T&   operator()(int i, int j);        // indexed access
  141.   T    operator()(int i, int j) const;  // indexed access
  143.   operator const T *(void) const { return m[0]; }  // array access
  144.   operator       T *(void)       { return m[0]; }
  146.   Pnt3 unproject(float u, float v, float z) const; // all input [-1,1]
  147.   Pnt3 unproject_fast(float u, float v, float z) const;
  149.   Xform<T> &operator*=(const Xform<T> &a); // (*this) = a * (*this)
  151.   // test how close to a rigid transformation the current
  152.   // transformation is, ideally returns 0
  153.   float test_rigidity(void);
  154.   // enforce the rigidity of the xform by setting the projective
  155.   // part to 0 0 0 1 and making sure rotation part is just a rotation
  156.   Xform<T> &enforce_rigidity(void);
  158. #ifndef __GNUC__
  159.   friend Xform<T> operator*(const Xform<T> &a,     // a*b
  160.     const Xform<T> &b); 
  161.   friend Xform<T> delta(Xform<T> a,                // d * a = b
  162. const Xform<T> &b); 
  163.   friend Xform<T> linear_interp(const Xform<T> &a, // r=t*a+(1-t)*b
  164. const Xform<T> &b,
  165. double          t);
  166.   friend Xform<T> bilinear_interp(const Xform<T> &u0v0,
  167.   const Xform<T> &u1v0,
  168.   const Xform<T> &u0v1,
  169.   const Xform<T> &u1v1,
  170.   double u, double v);
  171.   friend Xform<T> relative_xform(const Xform<T> &a, 
  172.  Xform<T> b); // c = b^-1 * a
  174.   friend ostream& operator<<(ostream &out, const Xform<T> &xf);
  175.   friend istream& operator>>(istream & in,       Xform<T> &xf);
  176. #else
  177.   friend Xform<T> operator*<>(const Xform<T> &a,     // a*b
  178.       const Xform<T> &b); 
  179.   friend Xform<T> delta<>(Xform<T> a,                // d * a = b
  180.   const Xform<T> &b); 
  181.   friend Xform<T> linear_interp<>(const Xform<T> &a, // r=t*a+(1-t)*b
  182.   const Xform<T> &b,
  183.   double          t);
  184.   friend Xform<T> bilinear_interp<>(const Xform<T> &u0v0,
  185.     const Xform<T> &u1v0,
  186.     const Xform<T> &u0v1,
  187.     const Xform<T> &u1v1,
  188.     double u, double v);
  189.   friend Xform<T> relative_xform<>(const Xform<T> &a, 
  190.    Xform<T> b); // c = b^-1 * a
  192.   friend ostream& operator<< <> (ostream &out, const Xform<T> &xf);
  193.   friend istream& operator>> <> (istream & in,       Xform<T> &xf);
  194. #endif
  195. };
  199. template <class T> inline
  200. Xform<T>::Xform(void) 
  201. {
  202.   m[1][0] = m[2][0] = m[3][0] = m[0][1] = m[2][1] = m[3][1] = 0.0;
  203.   m[0][2] = m[1][2] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0;
  204.   m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;
  205. }
  207. template <class T> inline
  208. Xform<T>::Xform(const double *a) 
  209. {
  210.   m[0][0] = a[0]; m[0][1] = a[1]; m[0][2] = a[2]; m[0][3] = a[3];
  211.   m[1][0] = a[4]; m[1][1] = a[5]; m[1][2] = a[6]; m[1][3] = a[7];
  212.   m[2][0] = a[8]; m[2][1] = a[9]; m[2][2] = a[10];m[2][3] = a[11];
  213.   m[3][0] = a[12];m[3][1] = a[13];m[3][2] = a[14];m[3][3] = a[15];
  214. }
  216. template <class T> inline
  217. Xform<T>::Xform(const float *a) 
  218. {
  219.   m[0][0] = a[0]; m[0][1] = a[1]; m[0][2] = a[2]; m[0][3] = a[3];
  220.   m[1][0] = a[4]; m[1][1] = a[5]; m[1][2] = a[6]; m[1][3] = a[7];
  221.   m[2][0] = a[8]; m[2][1] = a[9]; m[2][2] = a[10];m[2][3] = a[11];
  222.   m[3][0] = a[12];m[3][1] = a[13];m[3][2] = a[14];m[3][3] = a[15];
  223. }
  225. template <class T> inline
  226. Xform<T>::Xform(const T r[3][3], const T t[3])
  227. {
  228.   m[0][0] = r[0][0]; m[0][1]=r[1][0]; m[0][2]=r[2][0]; m[0][3]=0.0;
  229.   m[1][0] = r[0][1]; m[1][1]=r[1][1]; m[1][2]=r[2][1]; m[1][3]=0.0;
  230.   m[2][0] = r[0][2]; m[2][1]=r[1][2]; m[2][2]=r[2][2]; m[2][3]=0.0;
  231.   m[3][0] = t[0]; m[3][1] = t[1]; m[3][2] = t[2]; m[3][3] = 1.0;
  232. }
  235. template <class T> inline Xform<T> &
  236. Xform<T>::operator=(const double *a) 
  237. {
  238.   m[0][0] = a[0]; m[0][1] = a[1]; m[0][2] = a[2]; m[0][3] = a[3];
  239.   m[1][0] = a[4]; m[1][1] = a[5]; m[1][2] = a[6]; m[1][3] = a[7];
  240.   m[2][0] = a[8]; m[2][1] = a[9]; m[2][2] = a[10];m[2][3] = a[11];
  241.   m[3][0] = a[12];m[3][1] = a[13];m[3][2] = a[14];m[3][3] = a[15];
  242.   return *this;
  243. }
  245. template <class T> inline Xform<T> &
  246. Xform<T>::operator=(const float *a) 
  247. {
  248.   m[0][0] = a[0]; m[0][1] = a[1]; m[0][2] = a[2]; m[0][3] = a[3];
  249.   m[1][0] = a[4]; m[1][1] = a[5]; m[1][2] = a[6]; m[1][3] = a[7];
  250.   m[2][0] = a[8]; m[2][1] = a[9]; m[2][2] = a[10];m[2][3] = a[11];
  251.   m[3][0] = a[12];m[3][1] = a[13];m[3][2] = a[14];m[3][3] = a[15];
  252.   return *this;
  253. }
  255. template <class T> inline Xform<T> &
  256. Xform<T>::operator=(const Xform<T> &xf) 
  257. {
  258.   return operator=((const T *) xf);
  259. }
  261. // see Graphics Gems III p. 465
  262. // note quaternion order: w,x,y,z
  263. template <class T> inline Xform<T> &
  264. Xform<T>::fromQuaternion(const double q[4],
  265.  double tx, double ty, double tz)
  266. {
  267.   // q doesn't have to be unit
  268.   T norm2 = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
  269.   T s = (norm2 > 0.0) ? 2.0/norm2 : 0.0;
  270.   T xs = q[1]*s,   ys = q[2]*s,   zs = q[3]*s;
  271.   T wx = q[0]*xs,  wy = q[0]*ys,  wz = q[0]*zs;
  272.   T xx = q[1]*xs,  xy = q[1]*ys,  xz = q[1]*zs;
  273.   T yy = q[2]*ys,  yz = q[2]*zs,  zz = q[3]*zs;
  274.   
  275.   m[0][0] = 1.0 - (yy+zz);  m[0][1] = xy + wz;  m[0][2] = xz - wy;
  276.   m[1][0] = xy - wz;  m[1][1] = 1.0 - (xx+zz);  m[1][2] = yz + wx;
  277.   m[2][0] = xz + wy;  m[2][1] = yz - wx;  m[2][2] = 1.0 - (xx+yy);
  278.   
  279.   m[3][0] = tx;  m[3][1] = ty;  m[3][2] = tz;
  280.   m[0][3] = m[1][3] = m[2][3] = 0.0;
  281.   m[3][3] = 1.0;
  283.   return *this;
  284. }
  286. template <class T> inline Xform<T> &
  287. Xform<T>::fromQuaternion(const float q[4],
  288.  float tx, float ty, float tz)
  289. {
  290.   // q doesn't have to be unit
  291.   T norm2 = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
  292.   T s = (norm2 > 0.0) ? 2.0/norm2 : 0.0;
  293.   T xs = q[1]*s,   ys = q[2]*s,   zs = q[3]*s;
  294.   T wx = q[0]*xs,  wy = q[0]*ys,  wz = q[0]*zs;
  295.   T xx = q[1]*xs,  xy = q[1]*ys,  xz = q[1]*zs;
  296.   T yy = q[2]*ys,  yz = q[2]*zs,  zz = q[3]*zs;
  297.   
  298.   m[0][0] = 1.0 - (yy+zz);  m[0][1] = xy + wz;  m[0][2] = xz - wy;
  299.   m[1][0] = xy - wz;  m[1][1] = 1.0 - (xx+zz);  m[1][2] = yz + wx;
  300.   m[2][0] = xz + wy;  m[2][1] = yz - wx;  m[2][2] = 1.0 - (xx+yy);
  302.   m[3][0] = tx;  m[3][1] = ty;  m[3][2] = tz;
  303.   m[0][3] = m[1][3] = m[2][3] = 0.0;
  304.   m[3][3] = 1.0;
  306.   return *this;
  307. }
  309. // From Graphics Gems IV p. 213 
  310. // note transposed matrix and quaternion order w,x,y,z
  311. // see also Graphics Gems II pp. 351 - 354
  312. // Ignore translation and projective parts.
  313. // This algorithm avoids near-zero divides by looking for a 
  314. // large component - first w, then x, y, or z.  
  315. // When the trace is greater than zero, |w| is greater than 1/2,
  316. // which is as small as a largest component can be.
  317. // Otherwise, the largest diagonal entry corresponds to the 
  318. // largest of |x|, |y|, or |z|, one of which must be larger 
  319. // than |w|, and at least 1/2.
  320. template <class T> inline void 
  321. Xform<T>::toQuaternion(double q[4]) const
  322. {
  323.   T tr = m[0][0] + m[1][1]+ m[2][2];
  324.   if (tr >= 0.0) {
  325.     T s = sqrt(tr + m[3][3]);
  326.     q[0] = s*0.5;
  327.     s = 0.5 / s;
  328.     q[1] = (m[1][2] - m[2][1]) * s;
  329.     q[2] = (m[2][0] - m[0][2]) * s;
  330.     q[3] = (m[0][1] - m[1][0]) * s;
  331.   } else {
  332.     int i = 0;
  333.     if (m[1][1] > m[0][0]) i = 1;
  334.     if (m[2][2] > m[i][i]) i = 2;
  335.     int j = (i+1)%3;
  336.     int k = (j+1)%3;
  337.     T s = sqrt(m[3][3] + (m[i][i] - (m[j][j] + m[k][k])));
  338.     q[i+1] = s * 0.5;
  339.     s = 0.5 / s;
  340.     q[j+1] = (m[i][j] + m[j][i]) * s;
  341.     q[k+1] = (m[i][k] + m[k][i]) * s;
  342.     q[0]   = (m[j][k] - m[k][j]) * s;
  343.   }
  344.   if (m[3][3] != 1.0) {
  345.     T tmp = 1.0/sqrt(m[3][3]);
  346.     q[0]*=tmp; q[1]*=tmp; q[2]*=tmp; q[3]*=tmp; 
  347.   }
  348. }
  350. template <class T> inline void 
  351. Xform<T>::toQuaternion(float q[4]) const
  352. {
  353.   T tr = m[0][0] + m[1][1]+ m[2][2];
  354.   if (tr >= 0.0) {
  355.     T s = sqrt(tr + m[3][3]);
  356.     q[0] = s*0.5;
  357.     s = 0.5 / s;
  358.     q[1] = (m[1][2] - m[2][1]) * s;
  359.     q[2] = (m[2][0] - m[0][2]) * s;
  360.     q[3] = (m[0][1] - m[1][0]) * s;
  361.   } else {
  362.     int i = 0;
  363.     if (m[1][1] > m[0][0]) i = 1;
  364.     if (m[2][2] > m[i][i]) i = 2;
  365.     int j = (i+1)%3;
  366.     int k = (j+1)%3;
  367.     T s = sqrt(m[3][3] + (m[i][i] - (m[j][j] + m[k][k])));
  368.     q[i+1] = s * 0.5;
  369.     s = 0.5 / s;
  370.     q[j+1] = (m[i][j] + m[j][i]) * s;
  371.     q[k+1] = (m[i][k] + m[k][i]) * s;
  372.     q[0]   = (m[j][k] - m[k][j]) * s;
  373.   }
  374.   if (m[3][3] != 1.0) {
  375.     T tmp = 1.0/sqrt(m[3][3]);
  376.     q[0]*=tmp; q[1]*=tmp; q[2]*=tmp; q[3]*=tmp; 
  377.   }
  378. }
  380. template <class T> inline Xform<T> &
  381. Xform<T>::addQuaternion(T q0, T q1, T q2, T q3, T tx, T ty, T tz)
  382. {
  383.   rotQ(q0,q1,q2,q3);
  384.   translate(tx,ty,tz);
  385.   return *this;
  386. }
  388. template <class T> inline Xform<T> &
  389. Xform<T>::addQuaternion(const float *q, int n)
  390. {
  391.   rotQ(q[0],q[1],q[2],q[3]);
  392.   if (n == 7)
  393.     translate(q[4],q[5],q[6]);
  394.   return *this;
  395. }
  397. template <class T> inline Xform<T> &
  398. Xform<T>::addQuaternion(const double *q, int n)
  399. {
  400.   rotQ(q[0],q[1],q[2],q[3]);
  401.   if (n == 7)
  402.     translate(q[4],q[5],q[6]);
  403.   return *this;
  404. }
  406. //
  407. // Euler angle stuff from Graphics Gems IV pp. 222-229
  408. //
  409. /*** Order type constants, constructors, extractors ***/
  410.     /* There are 24 possible conventions, designated by:    */
  411.     /*   o EulAxI = axis used initially     */
  412.     /*   o EulPar = parity of axis permutation     */
  413.     /*   o EulRep = repetition of initial axis as last     */
  414.     /*   o EulFrm = frame from which axes are taken     */
  415.     /* Axes I,J,K will be a permutation of X,Y,Z.     */
  416.     /* Axis H will be either I or K, depending on EulRep.   */
  417.     /* Frame S takes axes from initial static frame.     */
  418.     /* If ord = (AxI=X, Par=Even, Rep=No, Frm=S), then     */
  419.     /* {a,b,c,ord} means Rz(c)Ry(b)Rx(a), where Rz(c)v     */
  420.     /* rotates v around Z by c radians.     */
  421. #define EulFrmS      0
  422. #define EulFrmR      1
  423. #define EulFrm(ord)  ((unsigned)(ord)&1)
  424. #define EulRepNo     0
  425. #define EulRepYes    1
  426. #define EulRep(ord)  (((unsigned)(ord)>>1)&1)
  427. #define EulParEven   0
  428. #define EulParOdd    1
  429. #define EulPar(ord)  (((unsigned)(ord)>>2)&1)
  430. #define EulSafe      "00010200"
  431. #define EulNext      "01020001"
  432. #define EulAxI(ord)  ((int)(EulSafe[(((unsigned)(ord)>>3)&3)]))
  433. #define EulAxJ(ord)  ((int)(EulNext[EulAxI(ord)+(EulPar(ord)==EulParOdd)]))
  434. #define EulAxK(ord)  ((int)(EulNext[EulAxI(ord)+(EulPar(ord)!=EulParOdd)]))
  435. #define EulAxH(ord)  ((EulRep(ord)==EulRepNo)?EulAxK(ord):EulAxI(ord))
  436. // EulGetOrd unpacks all useful information about order 
  437. // simultaneously. 
  438. #define EulGetOrd(ord,i,j,k,n,s,f) {
  439.   unsigned o=ord;f=o&1;o>>=1;s=o&1;o>>=1;
  440.   n=o&1;o>>=1;i=EulSafe[o&3];j=EulNext[i+n];
  441.   k=EulNext[i+1-n];}
  442. // EulOrd creates an order value between 0 and 23 from 4-tuple 
  443. // choices.
  444. #define EulOrd(i,p,r,f)    (((((((i)<<1)+(p))<<1)+(r))<<1)+(f))
  445. // Static axes 
  446. #define EulOrdXYZs    EulOrd(0,EulParEven,EulRepNo,EulFrmS)
  447. #define EulOrdXYXs    EulOrd(0,EulParEven,EulRepYes,EulFrmS)
  448. #define EulOrdXZYs    EulOrd(0,EulParOdd,EulRepNo,EulFrmS)
  449. #define EulOrdXZXs    EulOrd(0,EulParOdd,EulRepYes,EulFrmS)
  450. #define EulOrdYZXs    EulOrd(1,EulParEven,EulRepNo,EulFrmS)
  451. #define EulOrdYZYs    EulOrd(1,EulParEven,EulRepYes,EulFrmS)
  452. #define EulOrdYXZs    EulOrd(1,EulParOdd,EulRepNo,EulFrmS)
  453. #define EulOrdYXYs    EulOrd(1,EulParOdd,EulRepYes,EulFrmS)
  454. #define EulOrdZXYs    EulOrd(2,EulParEven,EulRepNo,EulFrmS)
  455. #define EulOrdZXZs    EulOrd(2,EulParEven,EulRepYes,EulFrmS)
  456. #define EulOrdZYXs    EulOrd(2,EulParOdd,EulRepNo,EulFrmS)
  457. #define EulOrdZYZs    EulOrd(2,EulParOdd,EulRepYes,EulFrmS)
  458. // Rotating axes
  459. #define EulOrdZYXr    EulOrd(0,EulParEven,EulRepNo,EulFrmR)
  460. #define EulOrdXYXr    EulOrd(0,EulParEven,EulRepYes,EulFrmR)
  461. #define EulOrdYZXr    EulOrd(0,EulParOdd,EulRepNo,EulFrmR)
  462. #define EulOrdXZXr    EulOrd(0,EulParOdd,EulRepYes,EulFrmR)
  463. #define EulOrdXZYr    EulOrd(1,EulParEven,EulRepNo,EulFrmR)
  464. #define EulOrdYZYr    EulOrd(1,EulParEven,EulRepYes,EulFrmR)
  465. #define EulOrdZXYr    EulOrd(1,EulParOdd,EulRepNo,EulFrmR)
  466. #define EulOrdYXYr    EulOrd(1,EulParOdd,EulRepYes,EulFrmR)
  467. #define EulOrdYXZr    EulOrd(2,EulParEven,EulRepNo,EulFrmR)
  468. #define EulOrdZXZr    EulOrd(2,EulParEven,EulRepYes,EulFrmR)
  469. #define EulOrdXYZr    EulOrd(2,EulParOdd,EulRepNo,EulFrmR)
  470. #define EulOrdZYZr    EulOrd(2,EulParOdd,EulRepYes,EulFrmR)
  472. template <class T> inline void 
  473. Xform<T>::fromEuler(const T _ea[3], int order)
  474. {
  475.   T ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
  476.   int i,j,k,n,s,f;
  477.   EulGetOrd(order,i,j,k,n,s,f);
  478.   T ea[3];
  479.   if (f==EulFrmR) { ea[0]=_ea[2]; ea[1]=_ea[1]; ea[2]=_ea[0]; }
  480.   else            { ea[0]=_ea[0]; ea[1]=_ea[1]; ea[2]=_ea[2]; }
  481.   if (n==EulParOdd) {ea[0] = -ea[0]; ea[1] =-ea[1]; ea[2] =-ea[2];}
  482.   ti = ea[0];   tj = ea[1];   th = ea[2];
  483.   ci = cos(ti); cj = cos(tj); ch = cos(th);
  484.   si = sin(ti); sj = sin(tj); sh = sin(th);
  485.   cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
  486.   if (s==EulRepYes) {
  487.     m[i][i] = cj;     m[j][i] =  sj*si;    m[k][i] =  sj*ci;
  488.     m[i][j] = sj*sh;  m[j][j] = -cj*ss+cc; m[k][j] = -cj*cs-sc;
  489.     m[i][k] = -sj*ch; m[j][k] =  cj*sc+cs; m[k][k] =  cj*cc-ss;
  490.   } else {
  491.     m[i][i] = cj*ch; m[j][i] = sj*sc-cs; m[k][i] = sj*cc+ss;
  492.     m[i][j] = cj*sh; m[j][j] = sj*ss+cc; m[k][j] = sj*cs-sc;
  493.     m[i][k] = -sj;   m[j][k] = cj*si;    m[k][k] = cj*ci;
  494.   }
  495.   m[3][0]=m[3][1]=m[3][2]=m[0][3]=m[1][3]=m[2][3]=0.0; m[3][3]=1.0;
  496. }
  498. template <class T> inline void 
  499. Xform<T>::toEuler(T ea[3], int order)
  500. {
  501.   int i,j,k,n,s,f;
  502.   EulGetOrd(order,i,j,k,n,s,f);
  503.   if (s==EulRepYes) {
  504.     T sy = sqrt(m[j][i]*m[j][i] + m[k][i]*m[k][i]);
  505.     if (sy > 16*FLT_EPSILON) {
  506.       ea[0] = atan2(m[j][i], m[i][k]);
  507.       ea[1] = atan2(sy, m[i][i]);
  508.       ea[2] = atan2(m[i][j], -m[i][k]);
  509.     } else {
  510.       ea[0] = atan2(-m[k][j], m[j][j]);
  511.       ea[1] = atan2(sy, m[i][i]);
  512.       ea[2] = 0;
  513.     }
  514.   } else {
  515.     T cy = sqrt(m[i][i]*m[i][i] + m[i][j]*m[i][j]);
  516.     if (cy > 16*FLT_EPSILON) {
  517.       ea[0] = atan2(m[j][k], m[k][k]);
  518.       ea[1] = atan2(-m[i][k], cy);
  519.       ea[2] = atan2(m[i][j], m[i][i]);
  520.     } else {
  521.       ea[0] = atan2(-m[k][j], m[j][j]);
  522.       ea[1] = atan2(-m[i][k], cy);
  523.       ea[2] = 0;
  524.     }
  525.   }
  526.   if (n==EulParOdd) {ea[0] = -ea[0]; ea[1] = -ea[1]; ea[2]=-ea[2];}
  527.   if (f==EulFrmR)   {T t = ea[0]; ea[0] = ea[2]; ea[2] = t;}
  528. }
  531. template <class T> inline Xform<T> &
  532. Xform<T>::fromFrame(const Pnt3 &x, 
  533.     const Pnt3 &y,
  534.     const Pnt3 &z,
  535.     const Pnt3 &org)
  536. {
  537.   m[0][0] = x[0]; m[0][1] = x[1]; m[0][2] = x[2]; m[0][3] = 0.0;
  538.   m[1][0] = y[0]; m[1][1] = y[1]; m[1][2] = y[2]; m[1][3] = 0.0;
  539.   m[2][0] = z[0]; m[2][1] = z[1]; m[2][2] = z[2]; m[2][3] = 0.0;
  540.   m[3][0] = org[0]; m[3][1] = org[1]; m[3][2] = org[2]; m[3][3] = 1.0;
  541.   return *this;
  542. }
  544. template <class T> inline Xform<T> &
  545. Xform<T>::fromRotTrans(const T r[3][3], const T t[3])
  546. {
  547.   m[0][0] = r[0][0]; m[0][1]=r[1][0]; m[0][2]=r[2][0]; m[0][3]=0.0;
  548.   m[1][0] = r[0][1]; m[1][1]=r[1][1]; m[1][2]=r[2][1]; m[1][3]=0.0;
  549.   m[2][0] = r[0][2]; m[2][1]=r[1][2]; m[2][2]=r[2][2]; m[2][3]=0.0;
  550.   m[3][0] = t[0]; m[3][1] = t[1]; m[3][2] = t[2]; m[3][3] = 1.0;
  551.   return *this;
  552. }
  555. template <class T> inline bool
  556. Xform<T>::isIdentity (void) const
  557. {
  558.   for (int i = 0; i < 4; i++)
  559.     for (int j = 0; j < 4; j++)
  560.       if (m[i][j] != (float)(i == j))
  561. return false;
  563.   return true;
  564. }
  567. template <class T> inline Xform<T> & 
  568. Xform<T>::identity(void)
  569. {
  570.   m[1][0] = m[2][0] = m[3][0] = m[0][1] = m[2][1] = m[3][1] = 0.0;
  571.   m[0][2] = m[1][2] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0;
  572.   m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;
  573.   return *this;
  574. }
  577. template <class T> inline Xform<T> & 
  578. Xform<T>::fast_invert(void)
  579. {
  580.   T tmp;
  581.   // transpose the rotation part
  582.   tmp = m[0][1]; m[0][1] = m[1][0]; m[1][0] = tmp;
  583.   tmp = m[0][2]; m[0][2] = m[2][0]; m[2][0] = tmp;
  584.   tmp = m[1][2]; m[1][2] = m[2][1]; m[2][1] = tmp;
  585.   // translation (T' = - T * R^-1)
  586.   T tx = -m[3][0];
  587.   T ty = -m[3][1];
  588.   T tz = -m[3][2];
  589.   m[3][0] = tx * m[0][0] + ty * m[1][0] + tz * m[2][0];
  590.   m[3][1] = tx * m[0][1] + ty * m[1][1] + tz * m[2][1];
  591.   m[3][2] = tx * m[0][2] + ty * m[1][2] + tz * m[2][2];
  592.   return *this;
  593. }
  596. template <class T> inline Xform<T> & 
  597. Xform<T>::invert(void)
  598. {
  599.    // Compute inverse matrix in place using Gauss-Jordan elimination
  600.    // with full pivoting for better numerical stability.
  602.    int swap_col[4], swap_row[4];
  603.    int pivot_used[4] = {0, 0, 0, 0};
  604.    int maxc, maxr, i, r, c;
  605.    double max, temp, pivot_inv;
  606.    
  607.    for (i=0; i<4; i++) {
  609.       // Determine largest magnitude pivot element
  611.       max = 0.0;
  612.       for (r=0; r<4; r++) {
  613.          if (pivot_used[r] != 1) {
  614.     for (c=0; c<4; c++) {
  615.        if (pivot_used[c] == 0) {
  616.           if (fabs(m[r][c]) >= max) {
  617.              max = fabs(m[r][c]);
  618.              maxr = r;
  619.              maxc = c;
  620.           }
  621.        }
  622.                else if (pivot_used[c] > 1) {
  623.           cerr << "invert: Singular Matrix-1" << endl;
  624.           return *this;
  625.        }
  626.             }
  627.  }
  628.       }
  630.       pivot_used[maxc] += 1;
  632.       // Swap rows to get the pivot element on the diagonal
  634.       if (maxr != maxc)
  635.          for (c=0; c<4; c++)
  636.             swap(m[maxr][c], m[maxc][c]);
  638.       // We need to remember what we swapped, for reversing later...
  640.       swap_row[i] = maxr;
  641.       swap_col[i] = maxc;
  643.       // If the pivot value is zero, matrix is singular
  645.       if (m[maxc][maxc] == 0.0) {
  646.          cerr << "invert: Singular Matrix-2" << endl;
  647.          return *this;
  648.       }
  650.       // Divide the row by the pivot value
  652.       pivot_inv = 1.0 / m[maxc][maxc];
  653.       m[maxc][maxc] = 1.0;
  654.       for (c=0; c<4; c++)
  655.          m[maxc][c] *= pivot_inv;
  657.       // Reduce the other, non-pivot rows
  659.       for (r=0; r<4; r++)  {
  660.          if (r != maxc) {
  661.     temp = m[r][maxc];
  662.     m[r][maxc] = 0.0;
  663.     for (c=0; c<4; c++)
  664.                m[r][c] -= m[maxc][c] * temp;
  665.          }
  666.       }
  667.    }
  669.    // Fix things up by swapping columns back in reverse order
  670.    
  671.    for (i=3; i>=0; i--) {
  672.       if (swap_row[i] != swap_col[i])
  673.          for (r=0; r<4; r++)
  674.             swap(m[r][swap_row[i]], m[r][swap_col[i]]);
  675.    }
  677.    return *this;
  678. }
  681. template <class T> inline void 
  682. Xform<T>::translate(const double t[3]) // M <- T*M
  683. {
  684.   for (int i=0; i<4; i++) m[i][0] += t[0]*m[i][3];
  685.   for (i=0; i<4; i++)     m[i][1] += t[1]*m[i][3];
  686.   for (i=0; i<4; i++)     m[i][2] += t[2]*m[i][3];
  687. }
  689. template <class T> inline void 
  690. Xform<T>::translate(const float t[3]) // M <- T*M
  691. {
  692.   for (int i=0; i<4; i++) m[i][0] += t[0]*m[i][3];
  693.   for (i=0; i<4; i++)     m[i][1] += t[1]*m[i][3];
  694.   for (i=0; i<4; i++)     m[i][2] += t[2]*m[i][3];
  695. }
  697. template <class T> inline void 
  698. Xform<T>::translate(T x, T y, T z) // M <- T*M
  699. {
  700.   for (int i=0; i<4; i++) m[i][0] += x*m[i][3];
  701.   for (i=0; i<4; i++)     m[i][1] += y*m[i][3];
  702.   for (i=0; i<4; i++)     m[i][2] += z*m[i][3];
  703. }
  705. template <class T> inline void 
  706. Xform<T>::getTranslation(double t[3]) const
  707. {
  708.   t[0] = m[3][0];  t[1] = m[3][1];  t[2] = m[3][2];
  709. }
  711. template <class T> inline void 
  712. Xform<T>::getTranslation(float t[3]) const
  713. {
  714.   t[0] = m[3][0];  t[1] = m[3][1];  t[2] = m[3][2];
  715. }
  717. template <class T> inline void 
  718. Xform<T>::removeTranslation(void)
  719. {
  720.   m[3][0] = m[3][1] = m[3][2] = 0.0;
  721. }
  723. template <class T> inline void 
  724. Xform<T>::removeTranslation(double t[3])
  725. {
  726.   t[0] = m[3][0];  t[1] = m[3][1];  t[2] = m[3][2];
  727.   m[3][0] = m[3][1] = m[3][2] = 0.0;
  728. }
  730. template <class T> inline void 
  731. Xform<T>::removeTranslation(float t[3])
  732. {
  733.   t[0] = m[3][0];  t[1] = m[3][1];  t[2] = m[3][2];
  734.   m[3][0] = m[3][1] = m[3][2] = 0.0;
  735. }
  737. template <class T> inline void 
  738. Xform<T>::scale(const double s[3]) // M <- S*M
  739. {
  740.   m[0][0] *= s[0]; m[1][0] *= s[0]; m[2][0] *= s[0]; m[3][0] *= s[0];
  741.   m[0][1] *= s[1]; m[1][1] *= s[1]; m[2][1] *= s[1]; m[3][1] *= s[1];
  742.   m[0][2] *= s[2]; m[1][2] *= s[2]; m[2][2] *= s[2]; m[3][2] *= s[2];
  743. }
  745. template <class T> inline void 
  746. Xform<T>::scale(const float s[3]) // M <- S*M
  747. {
  748.   m[0][0] *= s[0]; m[1][0] *= s[0]; m[2][0] *= s[0]; m[3][0] *= s[0];
  749.   m[0][1] *= s[1]; m[1][1] *= s[1]; m[2][1] *= s[1]; m[3][1] *= s[1];
  750.   m[0][2] *= s[2]; m[1][2] *= s[2]; m[2][2] *= s[2]; m[3][2] *= s[2];
  751. }
  753. template <class T> inline void 
  754. Xform<T>::scale(T x, T y, T z) // M <- S*M
  755. {
  756.   m[0][0] *= x; m[1][0] *= x; m[2][0] *= x; m[3][0] *= x;
  757.   m[0][1] *= y; m[1][1] *= y; m[2][1] *= y; m[3][1] *= y;
  758.   m[0][2] *= z; m[1][2] *= z; m[2][2] *= z; m[3][2] *= z;
  759. }
  761. template <class T> inline void 
  762. Xform<T>::get_e(int i, T e[3]) const
  763. {
  764.   e[0] = m[i][0];  e[1] = m[i][1];  e[2] = m[i][2];
  765. }
  767. template <class T> inline Xform<T> &
  768. Xform<T>::rotX(T a) // M <- R*M
  769. {
  770.   double sa = sin(a);
  771.   double ca = cos(a);
  772.   double row1[4];
  773.   for (int i=0; i<4; i++) row1[i] = m[i][1]; // temp. copy
  774.   for (i=0; i<4; i++)     m[i][1] = ca*row1[i] - sa*m[i][2];
  775.   for (i=0; i<4; i++)     m[i][2] = sa*row1[i] + ca*m[i][2];
  776.   return *this;
  777. }
  779. template <class T> inline Xform<T> &
  780. Xform<T>::rotY(T a) // M <- R*M
  781. {
  782.   double sa = sin(a);
  783.   double ca = cos(a);
  784.   double row0[4];
  785.   for (int i=0; i<4; i++) row0[i] =  m[i][0]; // temp. copy
  786.   for (i=0; i<4; i++)     m[i][0] =  ca*row0[i] + sa*m[i][2];
  787.   for (i=0; i<4; i++)     m[i][2] = -sa*row0[i] + ca*m[i][2];
  788.   return *this;
  789. }
  791. template <class T> inline Xform<T> & 
  792. Xform<T>::rotZ(T a) // M <- R*M
  793. {
  794.   double sa = sin(a);
  795.   double ca = cos(a);
  796.   double row0[4];
  797.   for (int i=0; i<4; i++) row0[i] = m[i][0]; // temp. copy
  798.   for (i=0; i<4; i++)     m[i][0] = ca*row0[i] - sa*m[i][1];
  799.   for (i=0; i<4; i++)     m[i][1] = sa*row0[i] + ca*m[i][1];
  800.   return *this;
  801. }
  803. // rotate around (0,0,0)
  804. template <class T> inline Xform<T> & 
  805. Xform<T>::rot(T angle, T ax, T ay, T az) // M <- R*M
  806. {
  807.   T q[4];
  808.   q[0] = cos(angle/2.0);
  809.   T fact = sin(angle/2.0) / sqrt(ax*ax+ay*ay+az*az);
  810.   q[1] = ax*fact;
  811.   q[2] = ay*fact;
  812.   q[3] = az*fact;
  814.   Xform<T> xf;
  815.   xf.fromQuaternion(q);
  816.   *this *= xf;
  817.   return *this;
  818. }
  820. // rotate around (ox,oy,oz)
  821. template <class T> inline Xform<T> & 
  822. Xform<T>::rot(T angle, T ax, T ay, T az,
  823.       T ox, T oy, T oz) // M <- R*M
  824. {
  825.   T q[4];
  826.   q[0] = cos(angle/2.0);
  827.   T fact = sin(angle/2.0) / sqrt(ax*ax+ay*ay+az*az);
  828.   q[1] = ax*fact;
  829.   q[2] = ay*fact;
  830.   q[3] = az*fact;
  832.   Xform<T> xf;
  833.   xf.fromQuaternion(q);
  835.   // add the part that makes the xf rotation to happen
  836.   // around (ox,oy,oz)
  837.   xf.m[3][0] += ox - (xf.m[0][0]*ox+xf.m[1][0]*oy+xf.m[2][0]*oz);
  838.   xf.m[3][1] += oy - (xf.m[0][1]*ox+xf.m[1][1]*oy+xf.m[2][1]*oz);
  839.   xf.m[3][2] += oz - (xf.m[0][2]*ox+xf.m[1][2]*oy+xf.m[2][2]*oz);
  841.   *this *= xf;
  843.   return *this;
  844. }
  846. template <class T> inline Xform<T> & 
  847. Xform<T>::rotQ(T q0, T q1, T q2, T q3)   // M <- R*M
  848. {
  849.   T q[4];
  850.   q[0] = q0;  q[1] = q1;  q[2] = q2;  q[3] = q3;
  852.   Xform<T> xf;
  853.   xf.fromQuaternion(q);
  854.   *this *= xf;
  855.   return *this;
  856. }
  859. template <class T> inline void
  860. Xform<T>::get_rot(T &angle, T &ax, T &ay, T &az)
  861. {
  862.   T q[4];
  863.   toQuaternion(q);
  864.   T len = sqrt(q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
  865.   if (len == 0.0) {
  866.     angle = ax = ay = 0.0; az = 1.0;
  867.   } else {
  868.     angle = acos(q[0]) * 2.0;
  869.     T invlen = 1.0/len;
  870.     if (sin(angle*.5) < 0.0) {
  871.       ax = -q[1]*invlen;
  872.       ay = -q[2]*invlen;
  873.       az = -q[3]*invlen;
  874.     } else {
  875.       ax = q[1]*invlen;
  876.       ay = q[2]*invlen;
  877.       az = q[3]*invlen;
  878.     }
  879.   }
  880. }
  882. template <class T> inline void 
  883. Xform<T>::apply(const double i[3], double o[3]) const // o = M*i
  884. {
  885.   T invw = 
  886.     1.0 / (m[0][3]*i[0]+m[1][3]*i[1]+m[2][3]*i[2]+m[3][3]);
  887.   o[0] = (m[0][0]*i[0]+m[1][0]*i[1]+m[2][0]*i[2]+m[3][0])*invw;
  888.   o[1] = (m[0][1]*i[0]+m[1][1]*i[1]+m[2][1]*i[2]+m[3][1])*invw;
  889.   o[2] = (m[0][2]*i[0]+m[1][2]*i[1]+m[2][2]*i[2]+m[3][2])*invw;
  890. }
  892. template <class T> inline void 
  893. Xform<T>::apply(const float i[3], float o[3]) const // o = M*i
  894. {
  895.   T invw = 
  896.     1.0 / (m[0][3]*i[0]+m[1][3]*i[1]+m[2][3]*i[2]+m[3][3]);
  897.   o[0] = (m[0][0]*i[0]+m[1][0]*i[1]+m[2][0]*i[2]+m[3][0])*invw;
  898.   o[1] = (m[0][1]*i[0]+m[1][1]*i[1]+m[2][1]*i[2]+m[3][1])*invw;
  899.   o[2] = (m[0][2]*i[0]+m[1][2]*i[1]+m[2][2]*i[2]+m[3][2])*invw;
  900. }
  902. template <class T> inline void 
  903. Xform<T>::apply_inv(const double i[3], double o[3]) const // o = M^-1*i
  904. {
  905.   T tx = i[0] - m[3][0];
  906.   T ty = i[1] - m[3][1];
  907.   T tz = i[2] - m[3][2];
  908.   o[0] = m[0][0]*tx + m[0][1]*ty + m[0][2]*tz;
  909.   o[1] = m[1][0]*tx + m[1][1]*ty + m[1][2]*tz;
  910.   o[2] = m[2][0]*tx + m[2][1]*ty + m[2][2]*tz;
  911. }
  913. template <class T> inline void 
  914. Xform<T>::apply_inv(const float i[3],  float o[3]) const // o = M^-1*i
  915. {
  916.   T tx = i[0] - m[3][0];
  917.   T ty = i[1] - m[3][1];
  918.   T tz = i[2] - m[3][2];
  919.   o[0] = m[0][0]*tx + m[0][1]*ty + m[0][2]*tz;
  920.   o[1] = m[1][0]*tx + m[1][1]*ty + m[1][2]*tz;
  921.   o[2] = m[2][0]*tx + m[2][1]*ty + m[2][2]*tz;
  922. }
  924. template <class T> inline void 
  925. Xform<T>::operator()(double i[3]) const
  926. {
  927.   T o[3];
  928.   o[0] = i[0]; o[1] = i[1]; o[2] = i[2];
  929.   T invw = 
  930.     1.0 / (m[0][3]*o[0]+m[1][3]*o[1]+m[2][3]*o[2]+m[3][3]);
  931.   i[0] = (m[0][0]*o[0]+m[1][0]*o[1]+m[2][0]*o[2]+m[3][0])*invw;
  932.   i[1] = (m[0][1]*o[0]+m[1][1]*o[1]+m[2][1]*o[2]+m[3][1])*invw;
  933.   i[2] = (m[0][2]*o[0]+m[1][2]*o[1]+m[2][2]*o[2]+m[3][2])*invw;
  934. }
  936. template <class T> inline void 
  937. Xform<T>::operator()(float i[3]) const
  938. {
  939.   T o[3];
  940.   o[0] = i[0]; o[1] = i[1]; o[2] = i[2];
  941.   T invw = 
  942.     1.0 / (m[0][3]*o[0]+m[1][3]*o[1]+m[2][3]*o[2]+m[3][3]);
  943.   i[0] = (m[0][0]*o[0]+m[1][0]*o[1]+m[2][0]*o[2]+m[3][0])*invw;
  944.   i[1] = (m[0][1]*o[0]+m[1][1]*o[1]+m[2][1]*o[2]+m[3][1])*invw;
  945.   i[2] = (m[0][2]*o[0]+m[1][2]*o[1]+m[2][2]*o[2]+m[3][2])*invw;
  946. }
  948. template <class T> inline void 
  949. Xform<T>::apply_nrm(const double i[3], double o[3]) const // o = M*i
  950. {
  951.   double invw = 
  952.     1.0 / (m[0][3]*i[0]+m[1][3]*i[1]+m[2][3]*i[2]+m[3][3]);
  953.   o[0] = (m[0][0]*i[0]+m[1][0]*i[1]+m[2][0]*i[2])*invw;
  954.   o[1] = (m[0][1]*i[0]+m[1][1]*i[1]+m[2][1]*i[2])*invw;
  955.   o[2] = (m[0][2]*i[0]+m[1][2]*i[1]+m[2][2]*i[2])*invw;
  956. }
  958. template <class T> inline void 
  959. Xform<T>::apply_nrm(const float i[3], float o[3]) const // o = M*i
  960. {
  961.   double invw = 
  962.     1.0 / (m[0][3]*i[0]+m[1][3]*i[1]+m[2][3]*i[2]+m[3][3]);
  963.   o[0] = (m[0][0]*i[0]+m[1][0]*i[1]+m[2][0]*i[2])*invw;
  964.   o[1] = (m[0][1]*i[0]+m[1][1]*i[1]+m[2][1]*i[2])*invw;
  965.   o[2] = (m[0][2]*i[0]+m[1][2]*i[1]+m[2][2]*i[2])*invw;
  966. }
  968. template <class T> inline T&
  969. Xform<T>::operator()(int i, int j)
  970. {
  971.   return m[j][i];
  972. }
  974. template <class T> inline T
  975. Xform<T>::operator()(int i, int j) const
  976. {
  977.   return m[j][i];
  978. }
  980. template <class T> inline Pnt3 
  981. Xform<T>::unproject(float u, float v, float z) const
  982. {
  983.   float w =  1.0/(u*m[0][3]+v*m[1][3]+z*m[2][3]+m[3][3]);
  984.   return Pnt3((u*m[0][0]+v*m[1][0]+z*m[2][0]+m[3][0])*w,
  985.       (u*m[0][1]+v*m[1][1]+z*m[2][1]+m[3][1])*w,
  986.       (u*m[0][2]+v*m[1][2]+z*m[2][2]+m[3][2])*w);
  987. }
  989. template <class T> inline Pnt3 
  990. Xform<T>::unproject_fast(float u, float v, float z) const
  991. {
  992.   float w =  1.0/(z*m[2][3]+m[3][3]);
  993.   return Pnt3((u*m[0][0]+m[3][0])*w,
  994.       (u*m[1][1]+m[3][1])*w,
  995.       -w);
  996. }
  998. template <class T> inline Xform<T> &
  999. Xform<T>::operator*=(const Xform<T> &a) // (*this) = a * (*this)
  1000. {
  1001.   Xform<T> b((T*)this);
  1002.   for (int i=0; i<4; i++) {
  1003.     for (int j=0; j<4; j++) {
  1004.       m[j][i] = a.m[0][i] * b.m[j][0] + a.m[1][i] * b.m[j][1] +
  1005.         a.m[2][i] * b.m[j][2] + a.m[3][i] * b.m[j][3];
  1006.     }
  1007.   }
  1008.   return *this;
  1009. }
  1012. // test how close to a rigid transformation the current
  1013. // transformation is, ideally returns 0
  1014. template <class T> inline float
  1015. Xform<T>::test_rigidity(void)
  1016. {
  1017.   float ans = 0.0;
  1018.   // check the projective row
  1019.   ans = max(ans, fabsf(m[0][3]));
  1020.   ans = max(ans, fabsf(m[1][3]));
  1021.   ans = max(ans, fabsf(m[2][3]));
  1022.   ans = max(ans, fabsf(m[3][3]-1.0));
  1023.   // test the dots of rows with itself
  1024.   ans = max(ans, fabsf(dot(Pnt3(m[0]), Pnt3(m[0]))-1.0));
  1025.   ans = max(ans, fabsf(dot(Pnt3(m[1]), Pnt3(m[1]))-1.0));
  1026.   ans = max(ans, fabsf(dot(Pnt3(m[2]), Pnt3(m[2]))-1.0));
  1027.   // test the dots of rows with other rows
  1028.   ans = max(ans, fabsf(dot(Pnt3(m[0]), Pnt3(m[1]))));
  1029.   ans = max(ans, fabsf(dot(Pnt3(m[1]), Pnt3(m[0]))));
  1030.   ans = max(ans, fabsf(dot(Pnt3(m[2]), Pnt3(m[0]))));
  1031.   ans = max(ans, fabsf(dot(Pnt3(m[0]), Pnt3(m[2]))));
  1032.   ans = max(ans, fabsf(dot(Pnt3(m[1]), Pnt3(m[2]))));
  1033.   ans = max(ans, fabsf(dot(Pnt3(m[2]), Pnt3(m[1]))));
  1034.   return ans;
  1035. }
  1038. // force the rigidity of the xform by setting the projective
  1039. // part to 0 0 0 1 and making sure rotation part is just a rotation
  1040. template <class T> inline Xform<T> &
  1041. Xform<T>::enforce_rigidity(void)
  1042. {
  1043.   m[0][3] = m[1][3] = m[2][3] = 0.0;
  1044.   m[3][3] = 1.0;
  1045.   T q[4];
  1046.   toQuaternion(q);
  1047.   // fromQuaternion() normalizes q
  1048.   fromQuaternion(q, m[3][0], m[3][1], m[3][2]);
  1049.   return *this;
  1050. }
  1053. template <class T> inline Xform<T>
  1054. operator*(const Xform<T> &a, const Xform<T> &b)
  1055. { 
  1056.   Xform<T> out;
  1057.   for (int i=0; i<4; i++) {
  1058.     for (int j=0; j<4; j++) {
  1059.       out.m[j][i] = 
  1060. a.m[0][i] * b.m[j][0] + a.m[1][i] * b.m[j][1] +
  1061. a.m[2][i] * b.m[j][2] + a.m[3][i] * b.m[j][3];
  1062.     }
  1063.   }
  1064.   return out;
  1065. }
  1068. // delta * a = b
  1069. template <class T> inline Xform<T>
  1070. delta(Xform<T> a,
  1071.       const Xform<T> &b)
  1072. {
  1073.   return b * a.fast_invert();
  1074. }
  1076. // Assume the transform only has rotation and translation.
  1077. // Calculate a delta matrix that transforms a to b.
  1078. // For other parts than rotation, do just regular interpolation.
  1079. // For rotation, calculate the quaternion, rotation axis and
  1080. // angle, interpolate the angle, transform back.
  1081. template <class T> inline Xform<T>
  1082. linear_interp(const Xform<T> &a,
  1083.       const Xform<T> &b,
  1084.       double          t)
  1085. {
  1086.   Xform<T> d = delta(a,b);
  1087.   // make sure no projective or scaling part
  1088.   assert(d.m[0][3] == 0.0);
  1089.   assert(d.m[1][3] == 0.0);
  1090.   assert(d.m[2][3] == 0.0);
  1091.   assert(d.m[3][3] == 1.0);
  1092.   // interpolate translation
  1093.   T tx = d.m[3][0]*t;
  1094.   T ty = d.m[3][1]*t;
  1095.   T tz = d.m[3][2]*t;
  1096.   // interpolate rotation
  1097.   T angle, x,y,z;
  1098.   d.get_rot(angle, x,y,z);
  1099.   d.identity();
  1100.   d.rot(angle*t, x,y,z);
  1102.   d.m[3][0] = tx;
  1103.   d.m[3][1] = ty;
  1104.   d.m[3][2] = tz;
  1106.   return d*a;
  1107. }
  1109. template <class T> inline Xform<T>
  1110. bilinear_interp(const Xform<T> &u0v0,
  1111. const Xform<T> &u1v0,
  1112. const Xform<T> &u0v1,
  1113. const Xform<T> &u1v1,
  1114. double u, double v)
  1115. {
  1116.   return linear_interp(linear_interp(u0v0, u1v0, u),
  1117.        linear_interp(u0v1, u1v1, u), v);
  1118. }
  1120. template <class T> inline Xform<T> 
  1121. relative_xform(const Xform<T> &a, Xform<T> b)
  1122. {
  1123.   return b.fast_invert() * a;
  1124. }
  1126. template <class T> inline ostream&
  1127. operator<<(ostream &out, const Xform<T> &xf)
  1128. { 
  1129.   for (int i=0; i<4; i++) {
  1130.     for (int j=0; j<4; j++) {
  1131.       //out << xf(i,j) << " ";
  1132.       out << xf.m[j][i] << " ";
  1133.     }
  1134.     out << endl;
  1135.   }
  1136.   return out;
  1137. }
  1139. template <class T> inline istream&
  1140. operator>>(istream &in, Xform<T> &xf)
  1141. { 
  1142.   for (int i=0; i<4; i++) {
  1143.     for (int j=0; j<4; j++) {
  1144.       in >> ws >> xf.m[j][i];
  1145.     }
  1146.   }
  1147.   return in;
  1148. }
  1154. #endif
  1156. //
  1157. // testing part...
  1158. //
  1159. #if 0
  1161. #define SHOW(x) cout << #x " = " << x << endl
  1162. #define SHOW3(x) cout << #x " = " << x[0] << " " << x[1] << " " << x[2] << " " << endl
  1164. void 
  1165. main(void)
  1166. {
  1167.   Xform<double> xf;
  1168.   double a[3] = { 1, 2, 3 };
  1169.   double b[3] = { 3,-1, 2 };
  1170.   double c[3];
  1172.   xf.translate(a);
  1173.   SHOW3(a);
  1174.   xf.getTranslation(c);
  1175.   SHOW3(c);
  1176.   xf.apply(a,c);
  1177.   SHOW3(c);
  1178.   xf.apply(b,c);
  1179.   SHOW3(b);
  1180.   SHOW3(c);
  1182.   cout << endl << "Test Euler angle stuff" << endl;
  1183.   cout << "Rotate vector around X by 45 deg" << endl;
  1184.   SHOW3(a);
  1185.   xf.identity();
  1186.   xf.rotX(M_PI/4.0);
  1187.   xf.apply(a,c);
  1188.   SHOW3(c);
  1189.   cout << "Rotate vector around Y by 45 deg" << endl;
  1190.   SHOW3(a);
  1191.   xf.identity();
  1192.   xf.rotY(M_PI/4.0);
  1193.   xf.apply(a,c);
  1194.   SHOW3(c);
  1195.   cout << "Rotate vector around Z by 45 deg" << endl;
  1196.   SHOW3(a);
  1197.   xf.identity();
  1198.   xf.rotZ(M_PI/4.0);
  1199.   xf.apply(a,c);
  1200.   SHOW3(c);
  1201.   cout << endl;
  1202.   double ea[3] = { M_PI/8.0, M_PI/7.0, M_PI/6.0 };
  1203.   SHOW3(ea);
  1204.   xf.fromEuler(ea, EulOrdXYZs);
  1205.   cout << "fromEuler()" << endl;
  1206.   SHOW(xf);
  1207.   xf.identity();
  1208.   xf.rotX(ea[0]);
  1209.   xf.rotY(ea[1]);
  1210.   xf.rotZ(ea[2]);
  1211.   SHOW(xf);
  1212.   xf.toEuler(ea, EulOrdXYZs);
  1213.   SHOW3(ea);
  1215.   cout << endl << "Test quaternions: " << endl;
  1216.   double q[4] = { .5, .5, .5, .5 };
  1217.   cout << q[0] << " "<< q[1] << " "<< q[2] << " "<< q[3] << endl;
  1218.   xf.fromQuaternion(q);
  1219.   cout << xf << endl;
  1220.   q[0] = q[1] = q[2] = q[3] = 0.0;
  1221.   xf.toQuaternion(q);
  1222.   cout << q[0] << " "<< q[1] << " "<< q[2] << " "<< q[3] << endl;
  1223.   float fq[4] = { .5, .5, .5, .5 };
  1224.   cout << fq[0] << " "<< fq[1] << " "<< fq[2] << " "<< fq[3] << endl;
  1225.   xf.fromQuaternion(fq);
  1226.   cout << xf << endl;
  1227.   fq[0] = fq[1] = fq[2] = fq[3] = 0.0;
  1228.   xf.toQuaternion(fq);
  1229.   cout << fq[0] << " "<< fq[1] << " "<< fq[2] << " "<< fq[3] << endl;
  1231.   cout << endl << "Test multiplication and inversion" << endl;
  1232.   Xform<double> xa,xb;
  1233.   xa.rotZ(M_PI/4.0);
  1234.   xa.translate(1,2,3);
  1235.   SHOW(xa);
  1236.   xb = xa;
  1237.   xb.fast_invert();
  1238.   SHOW(xb);
  1239.   SHOW(xa*xb);
  1240.   SHOW(xb*xa);
  1241.   cout << endl << "Test rotation" << endl;
  1242.   double angle, ax, ay, az;
  1243.   xa.identity();
  1244.   xa.rotX(M_PI/4.0);
  1245.   SHOW(xa);
  1246.   xa.get_rot(angle, ax, ay, az);
  1247.   SHOW(angle);
  1248.   SHOW(ax);
  1249.   SHOW(ay);
  1250.   SHOW(az);
  1251.   xb.identity();
  1252.   xb.rot(M_PI/4.0, 1, 0, 0);
  1253.   SHOW(xb);
  1254.   xa.identity();
  1255.   xa.rotY(M_PI/4.0);
  1256.   SHOW(xa);
  1257.   xa.get_rot(angle, ax, ay, az);
  1258.   SHOW(angle);
  1259.   SHOW(ax);
  1260.   SHOW(ay);
  1261.   SHOW(az);
  1262.   xb.identity();
  1263.   xb.rot(M_PI/4.0, 0, 1, 0);
  1264.   SHOW(xb);
  1265.   xa.identity();
  1266.   xa.rotZ(M_PI/4.0);
  1267.   SHOW(xa);
  1268.   xa.get_rot(angle, ax, ay, az);
  1269.   SHOW(angle);
  1270.   SHOW(ax);
  1271.   SHOW(ay);
  1272.   SHOW(az);
  1273.   xb.identity();
  1274.   xb.rot(M_PI/4.0, 0, 0, 1);
  1275.   SHOW(xb);
  1276.   xa.identity();
  1277.   SHOW(xa.rot(-.34, 1, -1, 1));
  1278.   xa.get_rot(angle, ax, ay, az);
  1279.   SHOW(angle);
  1280.   SHOW(ax);
  1281.   SHOW(ay);
  1282.   SHOW(az);
  1284.   cout << endl << "Test interpolation" << endl;
  1285.   xa.identity();
  1286.   xa.rotZ(M_PI/4.0);
  1287.   xa.translate(1,2,3);
  1288.   SHOW(xa);
  1289.   xb.identity();
  1290.   xb.rotY(-M_PI/4.0);
  1291.   xb.translate(-3,-2,-1);
  1292.   SHOW(xb);
  1293.   for (int i=0; i<=10; i++) {
  1294.     xf = linear_interp(xa,xb,i/10.0);
  1295.     SHOW(xf);
  1296.     a[0]=1;a[1]=1;a[2]=1;
  1297.     xf.apply(a,b);
  1298.     SHOW3(b);
  1299.   }
  1300. }
  1301. #endif