mv.h
Go to the documentation of this file.
00001 
00002 //
00003 //  Copyright 1997 Mitsubishi Electric Information Technology Center
00004 //  America (MEITCA).  All Rights Reserved.
00005 //
00006 //  Permission to use, copy, modify and distribute this software and
00007 //  its documentation for educational, research and non-profit
00008 //  purposes, without fee, and without a written agreement is hereby
00009 //  granted, provided that the above copyright notice and the
00010 //  following three paragraphs appear in all copies.
00011 //
00012 //  Permission to incorporate this software into commercial products
00013 //  may be obtained from MERL - A Mitsubishi Electric Research Lab, 201
00014 //  Broadway, Cambridge, MA 02139.
00015 //
00016 //  IN NO EVENT SHALL MEITCA BE LIABLE TO ANY PARTY FOR DIRECT,
00017 //  INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING
00018 //  LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
00019 //  DOCUMENTATION, EVEN IF MEITCA HAS BEEN ADVISED OF THE POSSIBILITY
00020 //  OF SUCH DAMAGES.
00021 //
00022 //  MEITCA SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
00023 //  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 //  FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS ON
00025 //  AN "AS IS" BASIS, AND MEITCA HAS NO OBLIGATIONS TO PROVIDE
00026 //  MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
00027 //
00028 //  Author:
00029 //    Brian Mirtich
00030 //    mirtich@merl.com
00031 //    617.621.7573
00032 //    www.merl.com/people/mirtich
00033 //
00035 
00036 
00037 
00038 #ifndef MV_H
00039 #define MV_H
00040 
00041 #include <iostream>
00042 #include <math.h>
00043 
00044 #if INVENTOR
00045 #include <Inventor/SbLinear.h>
00046 #include <Inventor/nodes/SoTransform.h>
00047 #endif
00048 
00049 #if OPCOUNTS
00050 #include "OpCounter.h"
00051 #endif
00052 
00053 using namespace std;
00054 namespace Vclip {
00056 //
00057 //  Matrix-Vector library 
00058 //
00060 
00061 /*
00062 
00063   SGI Inventor Interface
00064   ----------------------
00065   The compiler option -DINVENTOR causes compilation of functions
00066   for translating to and from the Inventor Library's vector and matrix
00067   types.
00068 
00069 
00070   Counting Operations
00071   -------------------
00072   To count floating point operations using the OpCounter package, use
00073   the compiler flag -DOPCOUNTS.  This will cause type Real to be
00074   compiled as type Double, which is like type double, but also has
00075   mechanisms for counting operations.  See OpCounter.h for details.
00076   Note: This functionality is not included with the V-Clip
00077   distribution.
00078 
00079 
00080   Implied Operands
00081   ----------------
00082   For most operations, the result is returned in 'this'.  For example:
00083  
00084   Vect3 a,b,c;
00085   ...
00086   a.add(b,c);  // a <- vector sum of b & c
00087 
00088   In the case of vector add, it does not matter if one of the operands
00089   is also the destination:  a.add(a,b) simply adds b to a, as
00090   expected.  However, for other operations, the destination can not be
00091   passed as an operand, as this will invalidate the computation:
00092 
00093   Mat3 M, N;
00094   ...
00095   M.mult(M, N);  // WRONG!  M will not be matrix product M N upon return!
00096 
00097   To fix this problem, such functions have versions that assume the
00098   destination as an implied operand, and take care to compute the
00099   result correctly in this situation.  The above incorrect call coan
00100   be replaced with
00101 
00102   M.postmult(N);  // M <- matrix product M N
00103 
00104   For consistency, and because it reduces parameter passing, implied
00105   operand versions of most operations exist, even when the destination
00106   could be passed as an operand without a problem.  For example:
00107 
00108   Vect3 a,b,c;
00109   ...
00110   a.add(b);  // a <- vector sum of a & b
00111 
00112   All problems associated with using destinations as operands can be
00113   avoided by following one simple rule: 
00114 
00115        *** NEVER EXPLICITLY PASS THE DESTINATION AS AN OPERAND. ***
00116 
00117   If you are trying to do this, chances are there is a different
00118   version of the function which does not require the destination to be
00119   explicitly passed.  For example,
00120 
00121   MatX M, N;
00122   ...
00123   M.mult(N, M);  // No good!  Desination M passed as explicit operand
00124   M.premult(N);  // Ok.  M <- N M  (M is an implicit operand)
00125   ...
00126   M.invert(M);   // No good!  Desination M passed as explicit operand
00127   M.invert();    // Ok.  M <- M^-1 (M is an implicit operand)
00128 
00129   Violating the rule is sometimes ok, but adhere to it, and never go
00130   wrong.  There are a few cases which still pose problems.  For
00131   instance, one can't square a matrix M in place.  M.mult(M,M)
00132   violates the rule, but so does M.premult(M).  Here, one must compute
00133   the square into a new matrix: S.mult(M,M).
00134 
00135 
00136   Transforming Vectors and Points
00137   -------------------------------
00138   The transformation operations differ slightly from the other
00139   operations in that the result is not returned via 'this'.  Instead,
00140   'this' is the transformation description (e.g. a rotation matrix),
00141   and the source and destination vectors/points are passed as
00142   operands, in that order.  There are also implied operand versions
00143   that only take a single vector/point argument, transforming it and
00144   storing the result in the same place.  For example:
00145 
00146   Se3 T;
00147   Vect3 v, xv;
00148   ...
00149   T.xformVect(v, xv);  // xv <- transformed version of v
00150   T.xformVect(v);      // v  <- transformed version of v
00151 
00152   For some transformation objects, calls like xformVect(v, v) are ok,
00153   but for others, they are not - xformVect(v) must be used.  
00154   To ensure correct usage:
00155 
00156   *** NEVER PASS THE SAME SOURCE AND DESTINATION TO A TRANSFORM FUNCTION ***
00157 
00158 */
00159 
00160 #if OPCOUNTS
00161 typedef Double Real;
00162 #else
00163 typedef double Real;
00164 #endif
00165 
00166 
00167 class Vect3;
00168 class Mat3;
00169 class MatX;
00170 class Quat;
00171 class Se3;
00172 
00173 
00175 //
00176 //  class Vect3
00177 //
00179 
00180 
00181 class Vect3 {
00182 
00183   friend class Mat3;
00184   friend class MatX;
00185   friend class Quat;
00186   friend class Se3;
00187 
00188 public:
00189 
00190   Real x, y, z;
00191 
00192   // constructors //////////////////////////////////////////////////////////////
00193 
00194   Vect3() {}
00195   Vect3(Real x_, Real y_, Real z_) {set(x_, y_, z_);}
00196 
00197   // setters / accessors / translators /////////////////////////////////////////
00198 
00199   void set(Real x_, Real y_, Real z_) {x = x_; y = y_; z = z_;}
00200 
00201   //Real &coord(int i) {return *(&x + i);} // index-based access:  0=x, 1=y, 2=z.
00202 
00203   // index-based access:  0=x, 1=y, 2=z.
00204   const Real &operator[](int i) const {return *(&x + i);} 
00205         Real &operator[](int i)       {return *(&x + i);} 
00206 
00207 
00208 #if INVENTOR
00209   inline void set(const SbVec3f &v);
00210   inline void toSbVec3f(SbVec3f &v) const;
00211 #endif
00212 
00213   // input / output ////////////////////////////////////////////////////////////
00214 
00215   ostream &print(ostream &os) const;
00216 
00217   // Read vector from stream If the next string read is the single
00218   // character i, j, or k, the appropriate unit vector is returned.
00219   // Plus and minus signs may be optionally placed in front of these
00220   // codes, e.g. +i or -k
00221   istream &read(istream &is);
00222 
00223   // operations not returning a Vect3 //////////////////////////////////////////
00224 
00225   //inline int operator==(const Vect3 &u, const Vect3 &v);
00226   inline int operator==(const Vect3 &other) const;
00227   inline Real dot(const Vect3 &other) const;
00228   inline Real norm()  const;
00229   inline Real norm2() const;  // norm^2
00230   inline Real distance (const Vect3 &other) const;
00231   inline Real distance2(const Vect3 &other) const;  // distance^2
00232   inline Real min() const;
00233   inline Real max() const;
00234   inline Real minAbs() const;
00235   inline Real maxAbs() const;
00236   inline void swap(Vect3 &other);
00237   // for symmetric invocations:
00238   static Real dot      (const Vect3 &u, const Vect3 &v) {return u.dot      (v);}
00239   static Real distance (const Vect3 &u, const Vect3 &v) {return u.distance (v);}
00240   static Real distance2(const Vect3 &u, const Vect3 &v) {return u.distance2(v);}
00241   static void swap     (      Vect3 &u,       Vect3 &v) {       u.swap     (v);}
00242   
00243   // operations returning result via this //////////////////////////////////////
00244 
00245   // The operation result indicated by the comments is always returned
00246   // in this.  The symbol [!] indicates that this must be distinct
00247   // from all of the operands.  The three-argument crossAdd() is
00248   // slightly different: this must be distinct from u and v, but not
00249   // necessarily from w.
00250 
00251   inline void normalize(const Vect3 &v);            // v/|v|
00252   inline void normalize();                          // this/|this|
00253   inline void negate(const Vect3 &v);               // -v
00254   inline void negate();                             // -this
00255   inline void add(const Vect3 &u, const Vect3 &v);  // u + v
00256   inline void add(const Vect3 &v);                  // this + v
00257   inline void sub(const Vect3 &u, const Vect3 &v);  // u - v
00258   inline void sub(const Vect3 &v);                  // this - v
00259   inline void mult(const Vect3 &u, const Vect3 &v); // u * v (component-wise) 
00260   inline void mult(const Vect3 &v);                 // this * v (component-wise)
00261   inline void scale(const Vect3 &v, Real s);        // s * v
00262   inline void scale(Real s);                        // s * this
00263   inline void cross(const Vect3 &u, const Vect3 &v);// u x v  [!]
00264   inline void precross(const Vect3 &v);             // v x this  [!]
00265   inline void postcross(const Vect3 &v);            // this x v  [!]
00266   inline void crossAdd(const Vect3 &u, const Vect3 &v, const Vect3 &w);
00267                                                     // u x v + w [!]
00268   inline void crossAdd(const Vect3 &u, const Vect3 &v);
00269                                                     //  u x v + this [!]
00270   inline void displace(const Vect3 &v, const Vect3 &u, Real lambda); 
00271                                                     // v + lambda * u
00272   inline void displace(const Vect3 &u, Real lambda);// this + lambda * u
00273   inline void interpolate(const Vect3 &u, const Vect3 &v, Real lambda);  
00274                                                     // (1-lambda)*u + lambda*v
00275 
00276 
00277   // Vect3 constants ///////////////////////////////////////////////////////////
00278 
00279   static const Vect3 ZERO;
00280   static const Vect3 I;     // unit vector along +x axis
00281   static const Vect3 J;     // unit vector along +y axis
00282   static const Vect3 K;     // unit vector along +z axis
00283   static const Vect3 I_;    // unit vector along -x axis
00284   static const Vect3 J_;    // unit vector along -y axis
00285   static const Vect3 K_;    // unit vector along -z axis
00286 
00287 };
00288 
00289 
00290 
00292 //
00293 //  class Mat3
00294 //
00296 
00297 
00298 class Mat3 {
00299 
00300   friend class Quat;
00301   friend class MatX;
00302 
00303 private:
00304 
00305   // (stored in row-major order)
00306   Real xx, xy, xz,
00307        yx, yy, yz,
00308        zx, zy, zz;
00309 
00310 public:
00311 
00312   // constructors //////////////////////////////////////////////////////////////
00313 
00314   Mat3() {}
00315   Mat3(const Vect3 &diag, const Vect3 &sym) {set(diag, sym);}
00316   Mat3(const Vect3 &axis, Real angle, int normalizeAxis = 1)
00317     {set(axis, angle, normalizeAxis);}
00318   Mat3(const Quat &q) {set(q);}
00319 
00320 
00321   // setters / accessors ///////////////////////////////////////////////////////
00322 
00323   // make a symmetric matrix, given the diagonal and symmetric
00324   // (off-diagonal) elements in canonical order
00325   inline void set(const Vect3 &diag, const Vect3 &sym);
00326   // set Mat3 as a rotation of 'angle' radians about 'axis'
00327   // axis is automatically normalized unless normalizeAxis = 0
00328   inline void set(const Vect3 &axis, Real angle, int normalizeAxis = 1);
00329   void set(const Quat &q);
00330 
00331   // index-based access:  0=xrow, 1=yrow, 2=zrow.
00332   const Vect3 &operator[](int i) const {return *(((Vect3 *) &xx) + i);}
00333         Vect3 &operator[](int i)       {return *(((Vect3 *) &xx) + i);}
00334 
00335   // set matrix to the skew symmetric matrix corresponding to 'v X'
00336   inline void setSkew(const Vect3 &v);
00337 
00338   // for reading rows
00339   const Vect3 &xrow() const {return *((Vect3 *) &xx);}
00340   const Vect3 &yrow() const {return *((Vect3 *) &yx);}
00341   const Vect3 &zrow() const {return *((Vect3 *) &zx);}
00342   // for writing to rows
00343   Vect3 &xrow()  {return *((Vect3 *) &xx);}
00344   Vect3 &yrow()  {return *((Vect3 *) &yx);}
00345   Vect3 &zrow()  {return *((Vect3 *) &zx);}
00346 
00347   // for reading columns
00348   Vect3 xcol() const {return Vect3(xx, yx, zx);}
00349   Vect3 ycol() const {return Vect3(xy, yy, zy);}
00350   Vect3 zcol() const {return Vect3(xz, yz, zz);}
00351   // for writing to columns
00352   inline void setXcol(const Vect3 &v);
00353   inline void setYcol(const Vect3 &v);
00354   inline void setZcol(const Vect3 &v);
00355 
00356   // for reading a symmetric matrix
00357   Vect3 diag() const {return Vect3(xx, yy, zz);}
00358   Vect3 sym()  const {return Vect3(yz, zx, xy);}
00359 
00360 
00361   // input / output ////////////////////////////////////////////////////////////
00362 
00363   ostream& print(ostream &os) const;
00364 
00365   // operations not returning a Mat3 ///////////////////////////////////////////
00366 
00367   inline Real det() const;
00368 
00369   // operations returning result via this //////////////////////////////////////
00370 
00371   // The operation result indicated by the comments is always returned
00372   // in this.  The symbol [!] indicates that this must be distinct
00373   // from all of the operands.  The invert() methods are based on the
00374   // explicit inversion formula from determinants; there are faster
00375   // and more accurate ways.  The invert() methods return one if the
00376   // matrix was not invertible, otherwise zero.
00377 
00378   inline void xpose(const Mat3 &M);                   // M^T       [!]
00379   inline void xpose();                                // this^T
00380   inline void symmetrize(const Mat3 &M);              // M + M^T
00381   inline void symmetrize();                           // this + this^T
00382          int  invert(const Mat3 &M);                  // M^-1      [!]
00383          int  invert();                               // this^-1
00384   inline void negate(const Mat3 &M);                  // -M
00385   inline void negate();                               // -this
00386   inline void add(const Mat3 &M, const Mat3 &N);      // M + N
00387   inline void add(const Mat3 &M);                     // this + M
00388   inline void sub(const Mat3 &M, const Mat3 &N);      // M - N
00389   inline void sub(const Mat3 &M);                     // this - M
00390   inline void scale(const Mat3 &M, Real s);           // s * M
00391   inline void scale(Real s);                          // s * this
00392          void mult(const Mat3 &M, const Mat3 &N);     // M * N     [!]
00393          void premult(const Mat3 &M);                 // M * this  [!]
00394          void postmult(const Mat3 &M);                // this * M  [!]
00395 
00396   // Transforming Vect3s ///////////////////////////////////////////////////////
00397 
00398   inline void xform(const Vect3 &v, Vect3 &xv) const; // (this)(v) => xv; 
00399                                                       // v & xv must be distinct
00400   inline void xform(Vect3 &v) const;                  // (this)(v) => v
00401 
00402   // These are exactly like the above methods, except the inverse
00403   // transform this^-1 (= this^T) is used.  This can be thought of as
00404   // a row vector transformation, e.g.: (v^T)(this) => xv^T
00405   inline void invXform(const Vect3 &v, Vect3 &xv) const;
00406   inline void invXform(Vect3 &v) const;
00407 
00408 
00409   // Mat3 constants ////////////////////////////////////////////////////////////
00410 
00411   static const Mat3 ZERO;    // zero matrix
00412   static const Mat3 ID;      // identity matrix
00413 
00414 };
00415 
00416 
00418 //
00419 //  class MatX
00420 //
00422 
00423 
00424 class MatX {
00425 
00426   friend class Se3;
00427 
00428 private:
00429 
00430   Mat3 R;
00431   Vect3 d;
00432 
00433 public:
00434 
00435   // constructors //////////////////////////////////////////////////////////////
00436 
00437   MatX()                                {}
00438   MatX(const Mat3 &R_, const Vect3 &d_) {set(R_, d_);}
00439   MatX(const Se3 &T)                    {set(T);}
00440 
00441 
00442   // setters / accessors / translators /////////////////////////////////////////
00443   
00444   inline void set(const Mat3 &R_, const Vect3 &d_) {R = R_; d = d_;}
00445   inline void set(const Se3 &T);
00446 
00447   const Mat3  &rot()   const {return R;}
00448   const Vect3 &trans() const {return d;}
00449         Mat3  &rot()         {return R;}
00450         Vect3 &trans()       {return d;}
00451   
00452   // input / output ////////////////////////////////////////////////////////////
00453 
00454   inline ostream& print(ostream &os) const;
00455   inline istream& read(istream &is);
00456 
00457   // operations returning result via this //////////////////////////////////////
00458 
00459   // The operation result indicated by the comments is always returned
00460   // in this.  The symbol [!] indicates that this must be distinct
00461   // from all of the operands.
00462 
00463   void mult(const MatX &M, const MatX &N);    // M * N     [!]
00464   void premult(const MatX &M);                // M * this  [!]
00465   void postmult(const MatX &M);               // this * M  [!]
00466   void invert(const MatX &M);                 // M^-1      [!]
00467   void invert();                              // this^-1
00468 
00469   // Transforming Vect3s ///////////////////////////////////////////////////////
00470 
00471   // MatXs can transform elements of R^3 either as vectors or as
00472   // points.  The [!] indicates that the operands must be distinct.
00473 
00474   inline void xformVect(const Vect3 &v, Vect3 &xv) const; // this*(v 0)=>xv  [!]
00475   inline void xformVect(Vect3 &v) const;                  // this*(v 0)=>v
00476   inline void xformPoint(const Vect3 &p, Vect3 &xp) const;// this*(p 1)=>xp  [!]
00477   inline void xformPoint(Vect3 &p) const;                 // this*(p 1)=>p
00478 
00479   // These are exactly like the above methods, except the inverse
00480   // transform this^-1 is used.
00481   inline void invXformVect(const Vect3 &v, Vect3 &xv) const;
00482   inline void invXformVect(Vect3 &v) const;                 
00483   inline void invXformPoint(const Vect3 &p, Vect3 &xp) const;
00484   inline void invXformPoint(Vect3 &p) const;                 
00485 
00486 
00487   // MatX constants ////////////////////////////////////////////////////////////
00488 
00489   static const MatX ID;      // identity matrix
00490 
00491 };
00492 
00493 
00494 
00496 //
00497 //  class Quat
00498 //
00500 
00501 
00502 class Quat {
00503 
00504   friend class Mat3;
00505   friend class Se3;
00506 
00507 private:
00508 
00509   Real s_, x_, y_, z_;
00510 
00511 
00512 public:
00513 
00514   // constructors //////////////////////////////////////////////////////////////
00515 
00516 
00517   Quat() {}
00518   Quat(Real s, Real x, Real y, Real z) {set(s, x, y, z);}
00519   Quat(Real angle, const Vect3 &axis, int normalizeAxis = 1)
00520     {set(angle, axis, normalizeAxis);}
00521   Quat(const Mat3 &R) {set(R);}
00522 
00523   // setters / accessors / translators /////////////////////////////////////////
00524 
00525   void set(Real s, Real x, Real y, Real z) {s_=s; x_=x; y_=y; z_=z;}
00526   // set a quaternion to a rotation of 'angle' radians about 'axis'
00527   // the axis passed is automatically normalized unless normalizeAxis = 0
00528   void set(Real angle, const Vect3 &axis, int normalizeAxis = 1);
00529   void set(const Mat3 &R);
00530 #if INVENTOR
00531   inline void set(const SbRotation &R);
00532   inline void toSbRotation(SbRotation &R) const;
00533 #endif
00534 
00535   Real s() const {return s_;}
00536   Real x() const {return x_;}
00537   Real y() const {return y_;}
00538   Real z() const {return z_;}
00539   inline Vect3 axis() const;  // normalized axis of rotation
00540   inline Real angle() const;  // angle of rotation in radians, in range [0, 2pi)
00541 
00542   // input / output ////////////////////////////////////////////////////////////
00543 
00544   ostream& print(ostream &os) const;
00545 
00546   // operations not returning a Quat ///////////////////////////////////////////
00547 
00548   int operator==(const Quat &other)
00549     {return s_ == other.s_ && x_ == other.x_ 
00550          && y_ == other.y_ && z_ == other.z_;}
00551 
00552 
00553   // operations returning result via this //////////////////////////////////////
00554 
00555   // The operation result indicated by the comments is always returned
00556   // in this.  The symbol [!] indicates that this must be distinct
00557   // from all of the operands.
00558 
00559   inline void normalize(const Quat &q);               // q/|q|
00560   inline void normalize();                            // this/|this|
00561   inline void invert(const Quat &q);                  // q^-1
00562   inline void invert();                               // this^-1
00563   void mult(const Quat &p, const Quat &q);            // p * q     [!]
00564   void premult(const Quat &q);                        // q * this  [!]
00565   void postmult(const Quat &q);                       // this * q  [!]
00566 
00567   // Transforming Vect3s ///////////////////////////////////////////////////////
00568 
00569   void xform(const Vect3 &u, Vect3 &v) const;    // this (v 0) this^-1 => xv
00570   void xform(Vect3 &v) const;                    // this (v 0) this^-1 => v
00571 
00572   // These are exactly like the above methods, except the inverse
00573   // transform is used (i.e. the factors this and this^-1 are swapped).
00574   void invXform(const Vect3 &v, Vect3 &xv) const;
00575   void invXform(Vect3 &v) const;
00576 
00577 
00578   // miscellaneous /////////////////////////////////////////////////////////////
00579 
00580   // Return the quaternion derivatives in 'this', given a current
00581   // orientation q and body angular velocity w.  Note that what is
00582   // returned in 'this' is not a unit quaternion, but the derivative
00583   // of one!
00584   inline void deriv(const Quat &q, const Vect3 &w);
00585 
00586   // Quat constants ////////////////////////////////////////////////////////////
00587 
00588   static const Quat ID;   // identity quaternion
00589 };
00590 
00591 
00592 
00594 //
00595 //  class Se3
00596 //
00598 
00599 
00600 class Se3 {
00601 
00602   friend class MatX;
00603 
00604 private:
00605 
00606   Quat q;     // rotation component
00607   Vect3 d;    // translation component
00608 
00609 public:
00610 
00611   // constructors //////////////////////////////////////////////////////////////
00612 
00613 
00614   Se3() {}
00615   Se3(const Quat &q_, const Vect3 &d_) {set(q_, d_);}
00616   Se3(const MatX &X) {set(X);}
00617 
00618   // setters / accessors / translators /////////////////////////////////////////
00619 
00620   void set(const Quat &q_, const Vect3 &d_) {q = q_; d = d_;}
00621   void set(const MatX &X) {q.set(X.R); d = X.d;}
00622 #if INVENTOR
00623   inline void set(const SoTransform *T);
00624   inline void toSoTransform(SoTransform *T) const;
00625 #endif
00626 
00627   const Quat  &rot()   const {return q;}
00628   const Vect3 &trans() const {return d;}
00629         Quat  &rot()         {return q;}
00630         Vect3 &trans()       {return d;}
00631 
00632   // input / output ////////////////////////////////////////////////////////////
00633 
00634 
00635   inline ostream& print(ostream &os) const;
00636 
00637   // Read Se3 from input stream.  The Se3 specification must be
00638   // enclosed in curly brackets { }.  Se3's are built up from a
00639   // sequence of translation and rotation operations.  A translation
00640   // opereration is specified by "trans v", where v is the translation
00641   // vector.  A rotation operation is specified by: "rot a v", where a
00642   // is the scalar rotation in *degrees*, and v is the axis of
00643   // rotation (a vector).
00644 
00645   istream& read(istream &is);
00646 
00647   // operations returning result via this //////////////////////////////////////
00648 
00649   // The operation result indicated by the comments is always returned
00650   // in this.  The symbol [!] indicates that this must be distinct
00651   // from all of the operands.
00652 
00653   inline void mult(const Se3 &T, const Se3 &U);    // T * U     [!]
00654   inline void premult(const Se3 &T);               // T * this  [!]
00655   inline void postmult(const Se3 &T);              // this * T  [!]
00656   inline void invert(const Se3 &T);                // T^-1
00657   inline void invert();                            // this^-1
00658 
00659   // Transforming Vect3s ///////////////////////////////////////////////////////
00660 
00661   // Se3s can transform elements of R^3 either as vectors or as
00662   // points.  Multiple operands need not be distinct.
00663 
00664   inline void xformVect(const Vect3 &v, Vect3 &xv) const;  // this * (v 0) => xv
00665   inline void xformVect(Vect3 &v) const;                   // this * (v 0) => v
00666   inline void xformPoint(const Vect3 &p, Vect3 &xp) const; // this * (p 1) => xp
00667   inline void xformPoint(Vect3 &p) const;                  // this * (p 1) => p
00668 
00669   // These are exactly like the above methods, except the inverse
00670   // transform this^-1 is used.
00671   inline void invXformVect(const Vect3 &v, Vect3 &xv) const;
00672   inline void invXformVect(Vect3 &v) const;                 
00673   inline void invXformPoint(const Vect3 &p, Vect3 &xp) const;
00674   inline void invXformPoint(Vect3 &p) const;                 
00675 
00676 
00677   // Se3 constants /////////////////////////////////////////////////////////////
00678 
00679   static const Se3 ID;      // identity Se3
00680 };
00681 
00682 
00683 
00685 //
00686 //  stream operators
00687 //
00689 
00690 
00691 inline ostream &operator<<(ostream &os, const Vect3 &v) {return v.print(os);}
00692 inline ostream &operator<<(ostream &os, const Mat3 &M)  {return M.print(os);}
00693 inline ostream &operator<<(ostream &os, const MatX &X)  {return X.print(os);}
00694 inline ostream &operator<<(ostream &os, const Quat &q)  {return q.print(os);}
00695 inline ostream &operator<<(ostream &os, const Se3 &T)   {return T.print(os);}
00696 
00697 inline istream &operator>>(istream &os, Vect3 &v)       {return v.read(os);}
00698 inline istream &operator>>(istream &is, MatX &X)        {return X.read(is);}
00699 inline istream &operator>>(istream &is, Se3 & T)        {return T.read(is);}
00700 
00701 
00702 
00704 //
00705 //  inline function definitions
00706 //
00708 
00709 
00710 #if INVENTOR
00711 void Vect3::set(const SbVec3f &vec)
00712 {
00713   const float *v = vec.getValue();
00714   x = v[0]; y = v[1]; z = v[2];
00715 }
00716 #endif
00717 
00718 
00719 #if INVENTOR
00720 void Vect3::toSbVec3f(SbVec3f &vec) const
00721 {
00722 #if OPCOUNTS
00723   vec.setValue(x.toDouble(), y.toDouble(), z.toDouble());
00724 #else
00725   vec.setValue(x, y, z);
00726 #endif
00727 }
00728 #endif
00729 
00730 
00731 int Vect3::operator==(const Vect3 &other) const
00732 {
00733   return x == other.x && y == other.y && z == other.z;
00734 }
00735 
00736 
00737 Real Vect3::dot(const Vect3 &other) const
00738 {
00739   return x * other.x + y * other.y + z * other.z;
00740 }
00741 
00742 
00743 Real Vect3::norm() const
00744 {
00745   return sqrt(x * x + y * y + z * z);
00746 }
00747 
00748 
00749 Real Vect3::norm2() const
00750 {
00751   return (x * x + y * y + z * z);
00752 }
00753 
00754 
00755 Real Vect3::distance(const Vect3 &other) const
00756 {
00757   Vect3 w;
00758 
00759   w.sub(other, *this);
00760   return w.norm();
00761 }
00762 
00763 
00764 Real Vect3::distance2(const Vect3 &other) const
00765 {
00766   Vect3 w;
00767 
00768   w.sub(other, *this);
00769   return w.norm2();
00770 }
00771 
00772 
00773 Real Vect3::min() const
00774 {
00775   return (x <= y) ? ((x <= z) ? x : z) : ((y <= z) ? y : z);
00776 }
00777 
00778 
00779 Real Vect3::max() const
00780 {
00781   return (x >= y) ? ((x >= z) ? x : z) : ((y >= z) ? y : z);
00782 }
00783 
00784 
00785 Real Vect3::minAbs() const
00786 {
00787   Real ax, ay, az;
00788   
00789   ax = fabs(x);
00790   ay = fabs(y);
00791   az = fabs(z);
00792   return (ax <= ay) ? ((ax <= az) ? ax : az) : ((ay <= az) ? ay : az);
00793 }
00794 
00795 
00796 Real Vect3::maxAbs() const
00797 {
00798   Real ax, ay, az;
00799   
00800   ax = fabs(x);
00801   ay = fabs(y);
00802   az = fabs(z);
00803   return (ax >= ay) ? ((ax >= az) ? ax : az) : ((ay >= az) ? ay : az);
00804 }
00805 
00806 
00807 void Vect3::swap(Vect3 &other)
00808 {
00809   Vect3 tmp;
00810 
00811   tmp = *this;
00812   *this = other;
00813   other = tmp;
00814 }
00815 
00816 
00817 void Vect3::normalize(const Vect3 &v)
00818 {
00819   Real s;
00820 
00821   s = 1.0 / sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
00822   x = s * v.x;
00823   y = s * v.y;
00824   z = s * v.z;
00825 }
00826 
00827 
00828 void Vect3::normalize()
00829 {
00830   Real s;
00831 
00832   s = 1.0 / sqrt(x * x + y * y + z * z);
00833   x *= s;
00834   y *= s;
00835   z *= s;
00836 }
00837 
00838 
00839 void Vect3::negate(const Vect3 &v)
00840 {
00841   x = - v.x;
00842   y = - v.y;
00843   z = - v.z;
00844 }
00845 
00846 
00847 void Vect3::negate()
00848 {
00849   x = - x;
00850   y = - y;
00851   z = - z;
00852 }
00853 
00854 
00855 void Vect3::add(const Vect3 &u, const Vect3 &v)
00856 {
00857   x = u.x + v.x;
00858   y = u.y + v.y;
00859   z = u.z + v.z;
00860 }
00861 
00862 
00863 void Vect3::add(const Vect3 &v)
00864 {
00865   x += v.x;
00866   y += v.y;
00867   z += v.z;
00868 }
00869 
00870 
00871 void Vect3::sub(const Vect3 &u, const Vect3 &v)
00872 {
00873   x = u.x - v.x;
00874   y = u.y - v.y;
00875   z = u.z - v.z;
00876 }
00877 
00878 
00879 void Vect3::sub(const Vect3 &v)
00880 {
00881   x -= v.x;
00882   y -= v.y;
00883   z -= v.z;
00884 }
00885 
00886 
00887 void Vect3::mult(const Vect3 &u, const Vect3 &v)
00888 {
00889   x = u.x * v.x;
00890   y = u.y * v.y;
00891   z = u.z * v.z;
00892 }
00893 
00894 
00895 void Vect3::mult(const Vect3 &v)
00896 {
00897   x *= v.x;
00898   y *= v.y;
00899   z *= v.z;
00900 }
00901 
00902 
00903 void Vect3::scale(const Vect3 &v, Real s)
00904 {
00905   x = s * v.x;
00906   y = s * v.y;
00907   z = s * v.z;
00908 }
00909 
00910 
00911 void Vect3::scale(Real s)
00912 {
00913   x *= s;
00914   y *= s;
00915   z *= s;
00916 }
00917 
00918 
00919 
00920 void Vect3::cross(const Vect3 &u, const Vect3 &v)
00921 {
00922   x = u.y * v.z - u.z * v.y;
00923   y = u.z * v.x - u.x * v.z;
00924   z = u.x * v.y - u.y * v.x;
00925 }
00926 
00927 
00928 void Vect3::precross(const Vect3 &v)
00929 {
00930   Real ox, oy;
00931 
00932   ox = x;
00933   oy = y;
00934   x = v.y * z - v.z * oy;
00935   y = v.z * ox - v.x * z;
00936   z = v.x * oy - v.y * ox;
00937 }
00938 
00939 
00940 void Vect3::postcross(const Vect3 &v)
00941 {
00942   Real ox, oy;
00943 
00944   ox = x;
00945   oy = y;
00946   x = oy * v.z - z * v.y;
00947   y = z * v.x - ox * v.z;
00948   z = ox * v.y - oy * v.x;
00949 }
00950 
00951 
00952 void Vect3::crossAdd(const Vect3 &u, const Vect3 &v, const Vect3 &w)
00953 {
00954   x = u.y * v.z - u.z * v.y + w.x;
00955   y = u.z * v.x - u.x * v.z + w.y;
00956   z = u.x * v.y - u.y * v.x + w.z;
00957 }
00958 
00959 
00960 void Vect3::crossAdd(const Vect3 &u, const Vect3 &v)
00961 {
00962   x += u.y * v.z - u.z * v.y;
00963   y += u.z * v.x - u.x * v.z;
00964   z += u.x * v.y - u.y * v.x;
00965 }
00966 
00967 
00968 void Vect3::displace(const Vect3 &v, const Vect3 &u, Real lambda)
00969 {
00970   x = v.x + lambda * u.x;
00971   y = v.y + lambda * u.y;
00972   z = v.z + lambda * u.z;
00973 }
00974 
00975 
00976 void Vect3::displace(const Vect3 &u, Real lambda)
00977 {
00978   x += lambda * u.x;
00979   y += lambda * u.y;
00980   z += lambda * u.z;
00981 }
00982 
00983 
00984 void Vect3::interpolate(const Vect3 &u, const Vect3 &v, Real lambda)
00985 {
00986   Real lambda2 = 1.0 - lambda;
00987 
00988   x = lambda2 * u.x + lambda * v.x;
00989   y = lambda2 * u.y + lambda * v.y;
00990   z = lambda2 * u.z + lambda * v.z;
00991 }
00992 
00993 
00994 void Mat3::set(const Vect3 &diag, const Vect3 &sym)
00995 {
00996   xx = diag.x;
00997   yy = diag.y;
00998   zz = diag.z;
00999   yz = zy = sym.x;
01000   zx = xz = sym.y;
01001   xy = yx = sym.z;
01002 }
01003 
01004 
01005 void Mat3::set(const Vect3 &axis, Real angle, int normalizeAxis)
01006 {
01007   Quat q;
01008 
01009   q.set(angle, axis, normalizeAxis);
01010   set(q);
01011 }
01012 
01013 void Mat3::setXcol(const Vect3 &v)
01014 {
01015   xx = v.x;
01016   yx = v.y;
01017   zx = v.z;
01018 }
01019 
01020 
01021 void Mat3::setYcol(const Vect3 &v)
01022 {
01023   xy = v.x;
01024   yy = v.y;
01025   zy = v.z;
01026 }
01027 
01028 
01029 void Mat3::setZcol(const Vect3 &v)
01030 {
01031   xz = v.x;
01032   yz = v.y;
01033   zz = v.z;
01034 }
01035 
01036 
01037 void Mat3::setSkew(const Vect3 &v)
01038 {
01039   xx = yy = zz = 0.0;
01040   zy =  v.x;
01041   yz = -v.x;
01042   xz =  v.y;
01043   zx = -v.y;
01044   yx =  v.z;
01045   xy = -v.z;
01046 }
01047 
01048 
01049 Real Mat3::det() const
01050 {
01051   return  xx * (yy * zz - yz * zy)
01052         + xy * (yz * zx - yx * zz)
01053         + xz * (yx * zy - yy * zx);
01054 }
01055 
01056 
01057 void Mat3::xpose(const Mat3 &M)
01058 {
01059   xx = M.xx;
01060   xy = M.yx;
01061   xz = M.zx;
01062 
01063   yx = M.xy;
01064   yy = M.yy;
01065   yz = M.zy;
01066 
01067   zx = M.xz;
01068   zy = M.yz;
01069   zz = M.zz;
01070 }
01071 
01072 
01073 void Mat3::xpose()
01074 {
01075   Real tmp;
01076 
01077   tmp = xy;
01078   xy = yx;
01079   yx = tmp;;
01080 
01081   tmp = yz;
01082   yz = zy;
01083   zy = tmp;
01084 
01085   tmp = zx;
01086   zx = xz;
01087   xz = tmp;
01088 }
01089 
01090 
01091 void Mat3::symmetrize(const Mat3 &M)
01092 {
01093   xx = 2 * M.xx;
01094   yy = 2 * M.yy;
01095   zz = 2 * M.zz;
01096   xy = yx = M.xy + M.yx;
01097   yz = zy = M.yz + M.zy;
01098   zx = xz = M.zx + M.xz;
01099 }
01100 
01101 
01102 void Mat3::symmetrize()
01103 {
01104   xx = 2 * xx;
01105   yy = 2 * yy;
01106   zz = 2 * zz;
01107   xy = yx = xy + yx;
01108   yz = zy = yz + zy;
01109   zx = xz = zx + xz;
01110 }
01111 
01112 
01113 void Mat3::negate(const Mat3 &M)
01114 {
01115   xx = - M.xx;
01116   xy = - M.xy;
01117   xz = - M.xz;
01118 
01119   yx = - M.yx;
01120   yy = - M.yy;
01121   yz = - M.yz;
01122 
01123   zx = - M.zx;
01124   zy = - M.zy;
01125   zz = - M.zz;
01126 }  
01127 
01128 
01129 void Mat3::negate()
01130 {
01131   xx = - xx;
01132   xy = - xy;
01133   xz = - xz;
01134 
01135   yx = - yx;
01136   yy = - yy;
01137   yz = - yz;
01138 
01139   zx = - zx;
01140   zy = - zy;
01141   zz = - zz;
01142 }  
01143 
01144 
01145 void Mat3::add(const Mat3 &M, const Mat3 &N)
01146 {
01147   xx = M.xx + N.xx;
01148   xy = M.xy + N.xy;
01149   xz = M.xz + N.xz;
01150 
01151   yx = M.yx + N.yx;
01152   yy = M.yy + N.yy;
01153   yz = M.yz + N.yz;
01154 
01155   zx = M.zx + N.zx;
01156   zy = M.zy + N.zy;
01157   zz = M.zz + N.zz;
01158 }
01159 
01160 
01161 void Mat3::add(const Mat3 &M)
01162 {
01163   xx += M.xx;
01164   xy += M.xy;
01165   xz += M.xz;
01166 
01167   yx += M.yx;
01168   yy += M.yy;
01169   yz += M.yz;
01170 
01171   zx += M.zx;
01172   zy += M.zy;
01173   zz += M.zz;
01174 }
01175 
01176 
01177 void Mat3::sub(const Mat3 &M, const Mat3 &N)
01178 {
01179   xx = M.xx - N.xx;
01180   xy = M.xy - N.xy;
01181   xz = M.xz - N.xz;
01182 
01183   yx = M.yx - N.yx;
01184   yy = M.yy - N.yy;
01185   yz = M.yz - N.yz;
01186 
01187   zx = M.zx - N.zx;
01188   zy = M.zy - N.zy;
01189   zz = M.zz - N.zz;
01190 }
01191 
01192 
01193 void Mat3::sub(const Mat3 &M)
01194 {
01195   xx -= M.xx;
01196   xy -= M.xy;
01197   xz -= M.xz;
01198 
01199   yx -= M.yx;
01200   yy -= M.yy;
01201   yz -= M.yz;
01202 
01203   zx -= M.zx;
01204   zy -= M.zy;
01205   zz -= M.zz;
01206 }
01207 
01208 
01209 void Mat3::scale(const Mat3 &M, Real s)
01210 {
01211   xx = s * M.xx; 
01212   xy = s * M.xy; 
01213   xz = s * M.xz;
01214   yx = s * M.yx; 
01215   yy = s * M.yy; 
01216   yz = s * M.yz;
01217   zx = s * M.zx; 
01218   zy = s * M.zy; 
01219   zz = s * M.zz;
01220 }
01221 
01222 
01223 void Mat3::scale(Real s)
01224 {
01225   xx *= s; 
01226   xy *= s; 
01227   xz *= s;
01228   yx *= s; 
01229   yy *= s; 
01230   yz *= s;
01231   zx *= s; 
01232   zy *= s; 
01233   zz *= s;
01234 }
01235 
01236 
01237 void Mat3::xform(const Vect3 &v, Vect3 &xv) const
01238 {
01239   xv.x = xx * v.x + xy * v.y + xz * v.z;
01240   xv.y = yx * v.x + yy * v.y + yz * v.z;
01241   xv.z = zx * v.x + zy * v.y + zz * v.z;
01242 }
01243 
01244 
01245 void Mat3::xform(Vect3 &v) const
01246 {
01247   Real ox, oy;
01248 
01249   ox = v.x; oy= v.y;
01250   v.x = xx * ox + xy * oy + xz * v.z;
01251   v.y = yx * ox + yy * oy + yz * v.z;
01252   v.z = zx * ox + zy * oy + zz * v.z;
01253 }
01254 
01255 
01256 void Mat3::invXform(const Vect3 &v, Vect3 &xv) const
01257 {
01258   xv.x = xx * v.x + yx * v.y + zx * v.z;
01259   xv.y = xy * v.x + yy * v.y + zy * v.z;
01260   xv.z = xz * v.x + yz * v.y + zz * v.z;
01261 }
01262 
01263 
01264 void Mat3::invXform(Vect3 &v) const
01265 {
01266   Real ox, oy;
01267 
01268   ox = v.x; oy= v.y;
01269   v.x = xx * ox + yx * oy + zx * v.z;
01270   v.y = xy * ox + yy * oy + zy * v.z;
01271   v.z = xz * ox + yz * oy + zz * v.z;
01272 }
01273 
01274 
01275 void MatX::set(const Se3 &T)
01276 {
01277   R.set(T.q); 
01278   d = T.d;
01279 }
01280 
01281 ostream& MatX::print(ostream &os) const
01282 {
01283   return os << R << d << endl;
01284 }
01285 
01286 
01287 istream& MatX::read(istream &is)
01288 {
01289   Se3 T;
01290   is >> T;
01291   set(T);
01292   return is;
01293 }
01294 
01295 void MatX::xformVect(const Vect3 &v, Vect3 &xv) const
01296 {
01297   R.xform(v, xv);
01298 }
01299 
01300   
01301 void MatX::xformVect(Vect3 &v) const
01302 {
01303   R.xform(v);
01304 }
01305 
01306   
01307 void MatX::xformPoint(const Vect3 &p, Vect3 &xp) const
01308 {
01309   R.xform(p, xp);
01310   xp.add(d);
01311 }
01312 
01313 
01314 void MatX::xformPoint(Vect3 &p) const
01315 {
01316   R.xform(p);
01317   p.add(d);
01318 }
01319 
01320 
01321 void MatX::invXformVect(const Vect3 &v, Vect3 &xv) const
01322 {
01323   R.invXform(v, xv);
01324 }
01325 
01326   
01327 void MatX::invXformVect(Vect3 &v) const
01328 {
01329   R.invXform(v);
01330 }
01331 
01332   
01333 void MatX::invXformPoint(const Vect3 &p, Vect3 &xp) const
01334 {
01335   xp.sub(p, d);
01336   R.invXform(xp);
01337 }
01338 
01339 
01340 void MatX::invXformPoint(Vect3 &p) const
01341 {
01342   p.sub(d);
01343   R.invXform(p);
01344 }
01345 
01346 
01347 #if INVENTOR
01348 void Quat::set(const SbRotation &rot)
01349 {
01350   const float *q = rot.getValue();
01351   s_ = q[3]; x_ = q[0]; y_ = q[1]; z_ = q[2];
01352 }
01353 #endif
01354 
01355 
01356 #if INVENTOR
01357 void Quat::toSbRotation(SbRotation &rot) const
01358 {
01359 #if OPCOUNTS
01360   rot.setValue(x_.toDouble(), y_.toDouble(), z_.toDouble(), s_.toDouble());
01361 #else
01362   rot.setValue(x_, y_, z_, s_);
01363 #endif
01364 }
01365 #endif
01366 
01367 
01368 Vect3 Quat::axis() const
01369 {
01370   Vect3 v(x_, y_, z_);
01371   if (v.norm() == 0.0) v = Vect3::I;  // axis is arbitrary here
01372   else v.normalize();
01373   return v;
01374 }
01375 
01376 
01377 Real Quat::angle() const
01378 {
01379 #if OPCOUNTS
01380   return 2 * acos(s_.toDouble());
01381 #else
01382   return 2 * acos(s_);
01383 #endif
01384 }
01385 
01386 
01387 void Quat::normalize(const Quat &q)
01388 {
01389   Real scale;
01390 
01391   scale = 1.0 / sqrt(q.s_*q.s_ + q.x_*q.x_ + q.y_*q.y_ + q.z_*q.z_);
01392   s_ = scale * q.s_;
01393   x_ = scale * q.x_;
01394   y_ = scale * q.y_;
01395   z_ = scale * q.z_;
01396 }
01397 
01398 
01399 void Quat::normalize()
01400 {
01401   Real scale;
01402 
01403   scale = 1.0 / sqrt(s_*s_ + x_*x_ + y_*y_ + z_*z_);
01404   s_ *= scale;
01405   x_ *= scale;
01406   y_ *= scale;
01407   z_ *= scale;
01408 }
01409 
01410 
01411 void Quat::invert(const Quat &q)
01412 {
01413   s_ = -q.s_;
01414   x_ =  q.x_;
01415   y_ =  q.y_;
01416   z_ =  q.z_;
01417 }
01418 
01419 
01420 void Quat::invert()
01421 {
01422   s_ = -s_;
01423 }
01424 
01425 
01426 void Quat::deriv(const Quat &q, const Vect3 &w)
01427 {
01428   s_ = 0.5 * (-q.x_ * w.x - q.y_ * w.y - q.z_ * w.z);
01429   x_ = 0.5 * ( q.s_ * w.x - q.z_ * w.y + q.y_ * w.z);
01430   y_ = 0.5 * ( q.z_ * w.x + q.s_ * w.y - q.x_ * w.z);
01431   z_ = 0.5 * (-q.y_ * w.x + q.x_ * w.y + q.s_ * w.z);
01432 }
01433 
01434 
01435 #if INVENTOR
01436 void Se3::set(const SoTransform *xform)
01437 {
01438   const float *quat;
01439   const float *trans;
01440 
01441   quat = xform->rotation.getValue().getValue();
01442   q.x_ = quat[0]; q.y_ = quat[1]; q.z_ = quat[2]; q.s_ = quat[3];
01443 
01444   trans = xform->translation.getValue().getValue();
01445   d.x = trans[0];
01446   d.y = trans[1];
01447   d.z = trans[2];
01448 }
01449 #endif
01450 
01451 
01452 #if INVENTOR
01453 void Se3::toSoTransform(SoTransform *xform) const
01454 {
01455 #if OPCOUNTS
01456   xform->rotation.setValue
01457     (q.x_.toDouble(), q.y_.toDouble(), q.z_.toDouble(), q.s_.toDouble());
01458   xform->translation.setValue(d.x.toDouble(), d.y.toDouble(), d.z.toDouble());
01459 #else
01460   xform->rotation.setValue(q.x_, q.y_, q.z_, q.s_);
01461   xform->translation.setValue(d.x, d.y, d.z);
01462 #endif
01463 }
01464 #endif
01465   
01466 
01467 ostream& Se3::print(ostream &os) const
01468 {
01469   return os << q << d;
01470 }
01471 
01472 
01473 void Se3::mult(const Se3 &T, const Se3 &U)
01474 {
01475   q.mult(T.q, U.q);
01476   T.q.xform(U.d, d);
01477   d.add(d, T.d);
01478 }
01479 
01480 
01481 void Se3::premult(const Se3 &T)
01482 {
01483   q.premult(T.q);
01484   T.q.xform(d);
01485   d.add(T.d);
01486 }
01487 
01488 
01489 void Se3::postmult(const Se3 &T)
01490 {
01491   Vect3 v;
01492 
01493   q.xform(T.d, v);
01494   d.add(v);
01495   q.postmult(T.q);
01496 }
01497 
01498 
01499 void Se3::invert(const Se3 &T)
01500 {
01501   q.s_ = -T.q.s_;
01502   q.x_ =  T.q.x_;
01503   q.y_ =  T.q.y_;
01504   q.z_ =  T.q.z_;
01505   q.xform(T.d, d);
01506   d.negate(d);
01507 }
01508 
01509 
01510 void Se3::invert()
01511 {
01512   q.s_ = -q.s_;
01513   q.xform(d);
01514   d.negate();
01515 }
01516 
01517 
01518 void Se3::xformVect(const Vect3 &v, Vect3 &xv) const
01519 {
01520   q.xform(v, xv);
01521 }
01522 
01523 
01524 void Se3::xformVect(Vect3 &v) const
01525 {
01526   q.xform(v);
01527 }
01528 
01529 
01530 void Se3::xformPoint(const Vect3 &p, Vect3 &xp) const
01531 {
01532   q.xform(p, xp);
01533   xp.add(d);
01534 }
01535 
01536 
01537 void Se3::xformPoint(Vect3 &p) const
01538 {
01539   q.xform(p);
01540   p.add(d);
01541 }
01542 
01543 
01544 void Se3::invXformVect(const Vect3 &v, Vect3 &xv) const
01545 {
01546   q.invXform(v, xv);
01547 }
01548 
01549 
01550 void Se3::invXformVect(Vect3 &v) const
01551 {
01552   q.invXform(v);
01553 }
01554 
01555 
01556 void Se3::invXformPoint(const Vect3 &p, Vect3 &xp) const
01557 {
01558   xp.sub(p, d);
01559   q.invXform(xp);
01560 }
01561 
01562 
01563 void Se3::invXformPoint(Vect3 &p) const
01564 {
01565   p.sub(d);
01566   q.invXform(p);
01567 }
01568 
01569 
01570 };
01571 #endif   // #ifndef MV_H
01572 
01573 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55