mv.h
Go to the documentation of this file.
1 //
3 // Copyright 1997 Mitsubishi Electric Information Technology Center
4 // America (MEITCA). All Rights Reserved.
5 //
6 // Permission to use, copy, modify and distribute this software and
7 // its documentation for educational, research and non-profit
8 // purposes, without fee, and without a written agreement is hereby
9 // granted, provided that the above copyright notice and the
10 // following three paragraphs appear in all copies.
11 //
12 // Permission to incorporate this software into commercial products
13 // may be obtained from MERL - A Mitsubishi Electric Research Lab, 201
14 // Broadway, Cambridge, MA 02139.
15 //
16 // IN NO EVENT SHALL MEITCA BE LIABLE TO ANY PARTY FOR DIRECT,
17 // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING
18 // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
19 // DOCUMENTATION, EVEN IF MEITCA HAS BEEN ADVISED OF THE POSSIBILITY
20 // OF SUCH DAMAGES.
21 //
22 // MEITCA SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
23 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON
25 // AN "AS IS" BASIS, AND MEITCA HAS NO OBLIGATIONS TO PROVIDE
26 // MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
27 //
28 // Author:
29 // Brian Mirtich
30 // mirtich@merl.com
31 // 617.621.7573
32 // www.merl.com/people/mirtich
33 //
35 
36 
37 
38 #ifndef MV_H
39 #define MV_H
40 
41 #include <iostream>
42 #include <math.h>
43 
44 #if INVENTOR
45 #include <Inventor/SbLinear.h>
46 #include <Inventor/nodes/SoTransform.h>
47 #endif
48 
49 #if OPCOUNTS
50 #include "OpCounter.h"
51 #endif
52 
53 using namespace std;
54 namespace Vclip {
56 //
57 // Matrix-Vector library
58 //
60 
61 /*
62 
63  SGI Inventor Interface
64  ----------------------
65  The compiler option -DINVENTOR causes compilation of functions
66  for translating to and from the Inventor Library's vector and matrix
67  types.
68 
69 
70  Counting Operations
71  -------------------
72  To count floating point operations using the OpCounter package, use
73  the compiler flag -DOPCOUNTS. This will cause type Real to be
74  compiled as type Double, which is like type double, but also has
75  mechanisms for counting operations. See OpCounter.h for details.
76  Note: This functionality is not included with the V-Clip
77  distribution.
78 
79 
80  Implied Operands
81  ----------------
82  For most operations, the result is returned in 'this'. For example:
83 
84  Vect3 a,b,c;
85  ...
86  a.add(b,c); // a <- vector sum of b & c
87 
88  In the case of vector add, it does not matter if one of the operands
89  is also the destination: a.add(a,b) simply adds b to a, as
90  expected. However, for other operations, the destination can not be
91  passed as an operand, as this will invalidate the computation:
92 
93  Mat3 M, N;
94  ...
95  M.mult(M, N); // WRONG! M will not be matrix product M N upon return!
96 
97  To fix this problem, such functions have versions that assume the
98  destination as an implied operand, and take care to compute the
99  result correctly in this situation. The above incorrect call coan
100  be replaced with
101 
102  M.postmult(N); // M <- matrix product M N
103 
104  For consistency, and because it reduces parameter passing, implied
105  operand versions of most operations exist, even when the destination
106  could be passed as an operand without a problem. For example:
107 
108  Vect3 a,b,c;
109  ...
110  a.add(b); // a <- vector sum of a & b
111 
112  All problems associated with using destinations as operands can be
113  avoided by following one simple rule:
114 
115  *** NEVER EXPLICITLY PASS THE DESTINATION AS AN OPERAND. ***
116 
117  If you are trying to do this, chances are there is a different
118  version of the function which does not require the destination to be
119  explicitly passed. For example,
120 
121  MatX M, N;
122  ...
123  M.mult(N, M); // No good! Desination M passed as explicit operand
124  M.premult(N); // Ok. M <- N M (M is an implicit operand)
125  ...
126  M.invert(M); // No good! Desination M passed as explicit operand
127  M.invert(); // Ok. M <- M^-1 (M is an implicit operand)
128 
129  Violating the rule is sometimes ok, but adhere to it, and never go
130  wrong. There are a few cases which still pose problems. For
131  instance, one can't square a matrix M in place. M.mult(M,M)
132  violates the rule, but so does M.premult(M). Here, one must compute
133  the square into a new matrix: S.mult(M,M).
134 
135 
136  Transforming Vectors and Points
137  -------------------------------
138  The transformation operations differ slightly from the other
139  operations in that the result is not returned via 'this'. Instead,
140  'this' is the transformation description (e.g. a rotation matrix),
141  and the source and destination vectors/points are passed as
142  operands, in that order. There are also implied operand versions
143  that only take a single vector/point argument, transforming it and
144  storing the result in the same place. For example:
145 
146  Se3 T;
147  Vect3 v, xv;
148  ...
149  T.xformVect(v, xv); // xv <- transformed version of v
150  T.xformVect(v); // v <- transformed version of v
151 
152  For some transformation objects, calls like xformVect(v, v) are ok,
153  but for others, they are not - xformVect(v) must be used.
154  To ensure correct usage:
155 
156  *** NEVER PASS THE SAME SOURCE AND DESTINATION TO A TRANSFORM FUNCTION ***
157 
158 */
159 
160 #if OPCOUNTS
161 typedef Double Real;
162 #else
163 typedef double Real;
164 #endif
165 
166 
167 class Vect3;
168 class Mat3;
169 class MatX;
170 class Quat;
171 class Se3;
172 
173 
175 //
176 // class Vect3
177 //
179 
180 
181 class Vect3 {
182 
183  friend class Mat3;
184  friend class MatX;
185  friend class Quat;
186  friend class Se3;
187 
188 public:
189 
190  Real x, y, z;
191 
192  // constructors //////////////////////////////////////////////////////////////
193 
194  Vect3() {}
195  Vect3(Real x_, Real y_, Real z_) {set(x_, y_, z_);}
196 
197  // setters / accessors / translators /////////////////////////////////////////
198 
199  void set(Real x_, Real y_, Real z_) {x = x_; y = y_; z = z_;}
200 
201  //Real &coord(int i) {return *(&x + i);} // index-based access: 0=x, 1=y, 2=z.
202 
203  // index-based access: 0=x, 1=y, 2=z.
204  const Real &operator[](int i) const {return *(&x + i);}
205  Real &operator[](int i) {return *(&x + i);}
206 
207 
208 #if INVENTOR
209  inline void set(const SbVec3f &v);
210  inline void toSbVec3f(SbVec3f &v) const;
211 #endif
212 
213  // input / output ////////////////////////////////////////////////////////////
214 
215  ostream &print(ostream &os) const;
216 
217  // Read vector from stream If the next string read is the single
218  // character i, j, or k, the appropriate unit vector is returned.
219  // Plus and minus signs may be optionally placed in front of these
220  // codes, e.g. +i or -k
221  istream &read(istream &is);
222 
223  // operations not returning a Vect3 //////////////////////////////////////////
224 
225  //inline int operator==(const Vect3 &u, const Vect3 &v);
226  inline int operator==(const Vect3 &other) const;
227  inline Real dot(const Vect3 &other) const;
228  inline Real norm() const;
229  inline Real norm2() const; // norm^2
230  inline Real distance (const Vect3 &other) const;
231  inline Real distance2(const Vect3 &other) const; // distance^2
232  inline Real min() const;
233  inline Real max() const;
234  inline Real minAbs() const;
235  inline Real maxAbs() const;
236  inline void swap(Vect3 &other);
237  // for symmetric invocations:
238  static Real dot (const Vect3 &u, const Vect3 &v) {return u.dot (v);}
239  static Real distance (const Vect3 &u, const Vect3 &v) {return u.distance (v);}
240  static Real distance2(const Vect3 &u, const Vect3 &v) {return u.distance2(v);}
241  static void swap ( Vect3 &u, Vect3 &v) { u.swap (v);}
242 
243  // operations returning result via this //////////////////////////////////////
244 
245  // The operation result indicated by the comments is always returned
246  // in this. The symbol [!] indicates that this must be distinct
247  // from all of the operands. The three-argument crossAdd() is
248  // slightly different: this must be distinct from u and v, but not
249  // necessarily from w.
250 
251  inline void normalize(const Vect3 &v); // v/|v|
252  inline void normalize(); // this/|this|
253  inline void negate(const Vect3 &v); // -v
254  inline void negate(); // -this
255  inline void add(const Vect3 &u, const Vect3 &v); // u + v
256  inline void add(const Vect3 &v); // this + v
257  inline void sub(const Vect3 &u, const Vect3 &v); // u - v
258  inline void sub(const Vect3 &v); // this - v
259  inline void mult(const Vect3 &u, const Vect3 &v); // u * v (component-wise)
260  inline void mult(const Vect3 &v); // this * v (component-wise)
261  inline void scale(const Vect3 &v, Real s); // s * v
262  inline void scale(Real s); // s * this
263  inline void cross(const Vect3 &u, const Vect3 &v);// u x v [!]
264  inline void precross(const Vect3 &v); // v x this [!]
265  inline void postcross(const Vect3 &v); // this x v [!]
266  inline void crossAdd(const Vect3 &u, const Vect3 &v, const Vect3 &w);
267  // u x v + w [!]
268  inline void crossAdd(const Vect3 &u, const Vect3 &v);
269  // u x v + this [!]
270  inline void displace(const Vect3 &v, const Vect3 &u, Real lambda);
271  // v + lambda * u
272  inline void displace(const Vect3 &u, Real lambda);// this + lambda * u
273  inline void interpolate(const Vect3 &u, const Vect3 &v, Real lambda);
274  // (1-lambda)*u + lambda*v
275 
276 
277  // Vect3 constants ///////////////////////////////////////////////////////////
278 
279  static const Vect3 ZERO;
280  static const Vect3 I; // unit vector along +x axis
281  static const Vect3 J; // unit vector along +y axis
282  static const Vect3 K; // unit vector along +z axis
283  static const Vect3 I_; // unit vector along -x axis
284  static const Vect3 J_; // unit vector along -y axis
285  static const Vect3 K_; // unit vector along -z axis
286 
287 };
288 
289 
290 
292 //
293 // class Mat3
294 //
296 
297 
298 class Mat3 {
299 
300  friend class Quat;
301  friend class MatX;
302 
303 private:
304 
305  // (stored in row-major order)
306  Real xx, xy, xz,
307  yx, yy, yz,
308  zx, zy, zz;
309 
310 public:
311 
312  // constructors //////////////////////////////////////////////////////////////
313 
314  Mat3() {}
315  Mat3(const Vect3 &diag, const Vect3 &sym) {set(diag, sym);}
316  Mat3(const Vect3 &axis, Real angle, int normalizeAxis = 1)
317  {set(axis, angle, normalizeAxis);}
318  Mat3(const Quat &q) {set(q);}
319 
320 
321  // setters / accessors ///////////////////////////////////////////////////////
322 
323  // make a symmetric matrix, given the diagonal and symmetric
324  // (off-diagonal) elements in canonical order
325  inline void set(const Vect3 &diag, const Vect3 &sym);
326  // set Mat3 as a rotation of 'angle' radians about 'axis'
327  // axis is automatically normalized unless normalizeAxis = 0
328  inline void set(const Vect3 &axis, Real angle, int normalizeAxis = 1);
329  void set(const Quat &q);
330 
331  // index-based access: 0=xrow, 1=yrow, 2=zrow.
332  const Vect3 &operator[](int i) const {return *(((Vect3 *) &xx) + i);}
333  Vect3 &operator[](int i) {return *(((Vect3 *) &xx) + i);}
334 
335  // set matrix to the skew symmetric matrix corresponding to 'v X'
336  inline void setSkew(const Vect3 &v);
337 
338  // for reading rows
339  const Vect3 &xrow() const {return *((Vect3 *) &xx);}
340  const Vect3 &yrow() const {return *((Vect3 *) &yx);}
341  const Vect3 &zrow() const {return *((Vect3 *) &zx);}
342  // for writing to rows
343  Vect3 &xrow() {return *((Vect3 *) &xx);}
344  Vect3 &yrow() {return *((Vect3 *) &yx);}
345  Vect3 &zrow() {return *((Vect3 *) &zx);}
346 
347  // for reading columns
348  Vect3 xcol() const {return Vect3(xx, yx, zx);}
349  Vect3 ycol() const {return Vect3(xy, yy, zy);}
350  Vect3 zcol() const {return Vect3(xz, yz, zz);}
351  // for writing to columns
352  inline void setXcol(const Vect3 &v);
353  inline void setYcol(const Vect3 &v);
354  inline void setZcol(const Vect3 &v);
355 
356  // for reading a symmetric matrix
357  Vect3 diag() const {return Vect3(xx, yy, zz);}
358  Vect3 sym() const {return Vect3(yz, zx, xy);}
359 
360 
361  // input / output ////////////////////////////////////////////////////////////
362 
363  ostream& print(ostream &os) const;
364 
365  // operations not returning a Mat3 ///////////////////////////////////////////
366 
367  inline Real det() const;
368 
369  // operations returning result via this //////////////////////////////////////
370 
371  // The operation result indicated by the comments is always returned
372  // in this. The symbol [!] indicates that this must be distinct
373  // from all of the operands. The invert() methods are based on the
374  // explicit inversion formula from determinants; there are faster
375  // and more accurate ways. The invert() methods return one if the
376  // matrix was not invertible, otherwise zero.
377 
378  inline void xpose(const Mat3 &M); // M^T [!]
379  inline void xpose(); // this^T
380  inline void symmetrize(const Mat3 &M); // M + M^T
381  inline void symmetrize(); // this + this^T
382  int invert(const Mat3 &M); // M^-1 [!]
383  int invert(); // this^-1
384  inline void negate(const Mat3 &M); // -M
385  inline void negate(); // -this
386  inline void add(const Mat3 &M, const Mat3 &N); // M + N
387  inline void add(const Mat3 &M); // this + M
388  inline void sub(const Mat3 &M, const Mat3 &N); // M - N
389  inline void sub(const Mat3 &M); // this - M
390  inline void scale(const Mat3 &M, Real s); // s * M
391  inline void scale(Real s); // s * this
392  void mult(const Mat3 &M, const Mat3 &N); // M * N [!]
393  void premult(const Mat3 &M); // M * this [!]
394  void postmult(const Mat3 &M); // this * M [!]
395 
396  // Transforming Vect3s ///////////////////////////////////////////////////////
397 
398  inline void xform(const Vect3 &v, Vect3 &xv) const; // (this)(v) => xv;
399  // v & xv must be distinct
400  inline void xform(Vect3 &v) const; // (this)(v) => v
401 
402  // These are exactly like the above methods, except the inverse
403  // transform this^-1 (= this^T) is used. This can be thought of as
404  // a row vector transformation, e.g.: (v^T)(this) => xv^T
405  inline void invXform(const Vect3 &v, Vect3 &xv) const;
406  inline void invXform(Vect3 &v) const;
407 
408 
409  // Mat3 constants ////////////////////////////////////////////////////////////
410 
411  static const Mat3 ZERO; // zero matrix
412  static const Mat3 ID; // identity matrix
413 
414 };
415 
416 
418 //
419 // class MatX
420 //
422 
423 
424 class MatX {
425 
426  friend class Se3;
427 
428 private:
429 
432 
433 public:
434 
435  // constructors //////////////////////////////////////////////////////////////
436 
437  MatX() {}
438  MatX(const Mat3 &R_, const Vect3 &d_) {set(R_, d_);}
439  MatX(const Se3 &T) {set(T);}
440 
441 
442  // setters / accessors / translators /////////////////////////////////////////
443 
444  inline void set(const Mat3 &R_, const Vect3 &d_) {R = R_; d = d_;}
445  inline void set(const Se3 &T);
446 
447  const Mat3 &rot() const {return R;}
448  const Vect3 &trans() const {return d;}
449  Mat3 &rot() {return R;}
450  Vect3 &trans() {return d;}
451 
452  // input / output ////////////////////////////////////////////////////////////
453 
454  inline ostream& print(ostream &os) const;
455  inline istream& read(istream &is);
456 
457  // operations returning result via this //////////////////////////////////////
458 
459  // The operation result indicated by the comments is always returned
460  // in this. The symbol [!] indicates that this must be distinct
461  // from all of the operands.
462 
463  void mult(const MatX &M, const MatX &N); // M * N [!]
464  void premult(const MatX &M); // M * this [!]
465  void postmult(const MatX &M); // this * M [!]
466  void invert(const MatX &M); // M^-1 [!]
467  void invert(); // this^-1
468 
469  // Transforming Vect3s ///////////////////////////////////////////////////////
470 
471  // MatXs can transform elements of R^3 either as vectors or as
472  // points. The [!] indicates that the operands must be distinct.
473 
474  inline void xformVect(const Vect3 &v, Vect3 &xv) const; // this*(v 0)=>xv [!]
475  inline void xformVect(Vect3 &v) const; // this*(v 0)=>v
476  inline void xformPoint(const Vect3 &p, Vect3 &xp) const;// this*(p 1)=>xp [!]
477  inline void xformPoint(Vect3 &p) const; // this*(p 1)=>p
478 
479  // These are exactly like the above methods, except the inverse
480  // transform this^-1 is used.
481  inline void invXformVect(const Vect3 &v, Vect3 &xv) const;
482  inline void invXformVect(Vect3 &v) const;
483  inline void invXformPoint(const Vect3 &p, Vect3 &xp) const;
484  inline void invXformPoint(Vect3 &p) const;
485 
486 
487  // MatX constants ////////////////////////////////////////////////////////////
488 
489  static const MatX ID; // identity matrix
490 
491 };
492 
493 
494 
496 //
497 // class Quat
498 //
500 
501 
502 class Quat {
503 
504  friend class Mat3;
505  friend class Se3;
506 
507 private:
508 
509  Real s_, x_, y_, z_;
510 
511 
512 public:
513 
514  // constructors //////////////////////////////////////////////////////////////
515 
516 
517  Quat() {}
518  Quat(Real s, Real x, Real y, Real z) {set(s, x, y, z);}
519  Quat(Real angle, const Vect3 &axis, int normalizeAxis = 1)
520  {set(angle, axis, normalizeAxis);}
521  Quat(const Mat3 &R) {set(R);}
522 
523  // setters / accessors / translators /////////////////////////////////////////
524 
525  void set(Real s, Real x, Real y, Real z) {s_=s; x_=x; y_=y; z_=z;}
526  // set a quaternion to a rotation of 'angle' radians about 'axis'
527  // the axis passed is automatically normalized unless normalizeAxis = 0
528  void set(Real angle, const Vect3 &axis, int normalizeAxis = 1);
529  void set(const Mat3 &R);
530 #if INVENTOR
531  inline void set(const SbRotation &R);
532  inline void toSbRotation(SbRotation &R) const;
533 #endif
534 
535  Real s() const {return s_;}
536  Real x() const {return x_;}
537  Real y() const {return y_;}
538  Real z() const {return z_;}
539  inline Vect3 axis() const; // normalized axis of rotation
540  inline Real angle() const; // angle of rotation in radians, in range [0, 2pi)
541 
542  // input / output ////////////////////////////////////////////////////////////
543 
544  ostream& print(ostream &os) const;
545 
546  // operations not returning a Quat ///////////////////////////////////////////
547 
548  int operator==(const Quat &other)
549  {return s_ == other.s_ && x_ == other.x_
550  && y_ == other.y_ && z_ == other.z_;}
551 
552 
553  // operations returning result via this //////////////////////////////////////
554 
555  // The operation result indicated by the comments is always returned
556  // in this. The symbol [!] indicates that this must be distinct
557  // from all of the operands.
558 
559  inline void normalize(const Quat &q); // q/|q|
560  inline void normalize(); // this/|this|
561  inline void invert(const Quat &q); // q^-1
562  inline void invert(); // this^-1
563  void mult(const Quat &p, const Quat &q); // p * q [!]
564  void premult(const Quat &q); // q * this [!]
565  void postmult(const Quat &q); // this * q [!]
566 
567  // Transforming Vect3s ///////////////////////////////////////////////////////
568 
569  void xform(const Vect3 &u, Vect3 &v) const; // this (v 0) this^-1 => xv
570  void xform(Vect3 &v) const; // this (v 0) this^-1 => v
571 
572  // These are exactly like the above methods, except the inverse
573  // transform is used (i.e. the factors this and this^-1 are swapped).
574  void invXform(const Vect3 &v, Vect3 &xv) const;
575  void invXform(Vect3 &v) const;
576 
577 
578  // miscellaneous /////////////////////////////////////////////////////////////
579 
580  // Return the quaternion derivatives in 'this', given a current
581  // orientation q and body angular velocity w. Note that what is
582  // returned in 'this' is not a unit quaternion, but the derivative
583  // of one!
584  inline void deriv(const Quat &q, const Vect3 &w);
585 
586  // Quat constants ////////////////////////////////////////////////////////////
587 
588  static const Quat ID; // identity quaternion
589 };
590 
591 
592 
594 //
595 // class Se3
596 //
598 
599 
600 class Se3 {
601 
602  friend class MatX;
603 
604 private:
605 
606  Quat q; // rotation component
607  Vect3 d; // translation component
608 
609 public:
610 
611  // constructors //////////////////////////////////////////////////////////////
612 
613 
614  Se3() {}
615  Se3(const Quat &q_, const Vect3 &d_) {set(q_, d_);}
616  Se3(const MatX &X) {set(X);}
617 
618  // setters / accessors / translators /////////////////////////////////////////
619 
620  void set(const Quat &q_, const Vect3 &d_) {q = q_; d = d_;}
621  void set(const MatX &X) {q.set(X.R); d = X.d;}
622 #if INVENTOR
623  inline void set(const SoTransform *T);
624  inline void toSoTransform(SoTransform *T) const;
625 #endif
626 
627  const Quat &rot() const {return q;}
628  const Vect3 &trans() const {return d;}
629  Quat &rot() {return q;}
630  Vect3 &trans() {return d;}
631 
632  // input / output ////////////////////////////////////////////////////////////
633 
634 
635  inline ostream& print(ostream &os) const;
636 
637  // Read Se3 from input stream. The Se3 specification must be
638  // enclosed in curly brackets { }. Se3's are built up from a
639  // sequence of translation and rotation operations. A translation
640  // opereration is specified by "trans v", where v is the translation
641  // vector. A rotation operation is specified by: "rot a v", where a
642  // is the scalar rotation in *degrees*, and v is the axis of
643  // rotation (a vector).
644 
645  istream& read(istream &is);
646 
647  // operations returning result via this //////////////////////////////////////
648 
649  // The operation result indicated by the comments is always returned
650  // in this. The symbol [!] indicates that this must be distinct
651  // from all of the operands.
652 
653  inline void mult(const Se3 &T, const Se3 &U); // T * U [!]
654  inline void premult(const Se3 &T); // T * this [!]
655  inline void postmult(const Se3 &T); // this * T [!]
656  inline void invert(const Se3 &T); // T^-1
657  inline void invert(); // this^-1
658 
659  // Transforming Vect3s ///////////////////////////////////////////////////////
660 
661  // Se3s can transform elements of R^3 either as vectors or as
662  // points. Multiple operands need not be distinct.
663 
664  inline void xformVect(const Vect3 &v, Vect3 &xv) const; // this * (v 0) => xv
665  inline void xformVect(Vect3 &v) const; // this * (v 0) => v
666  inline void xformPoint(const Vect3 &p, Vect3 &xp) const; // this * (p 1) => xp
667  inline void xformPoint(Vect3 &p) const; // this * (p 1) => p
668 
669  // These are exactly like the above methods, except the inverse
670  // transform this^-1 is used.
671  inline void invXformVect(const Vect3 &v, Vect3 &xv) const;
672  inline void invXformVect(Vect3 &v) const;
673  inline void invXformPoint(const Vect3 &p, Vect3 &xp) const;
674  inline void invXformPoint(Vect3 &p) const;
675 
676 
677  // Se3 constants /////////////////////////////////////////////////////////////
678 
679  static const Se3 ID; // identity Se3
680 };
681 
682 
683 
685 //
686 // stream operators
687 //
689 
690 
691 inline ostream &operator<<(ostream &os, const Vect3 &v) {return v.print(os);}
692 inline ostream &operator<<(ostream &os, const Mat3 &M) {return M.print(os);}
693 inline ostream &operator<<(ostream &os, const MatX &X) {return X.print(os);}
694 inline ostream &operator<<(ostream &os, const Quat &q) {return q.print(os);}
695 inline ostream &operator<<(ostream &os, const Se3 &T) {return T.print(os);}
696 
697 inline istream &operator>>(istream &os, Vect3 &v) {return v.read(os);}
698 inline istream &operator>>(istream &is, MatX &X) {return X.read(is);}
699 inline istream &operator>>(istream &is, Se3 & T) {return T.read(is);}
700 
701 
702 
704 //
705 // inline function definitions
706 //
708 
709 
710 #if INVENTOR
711 void Vect3::set(const SbVec3f &vec)
712 {
713  const float *v = vec.getValue();
714  x = v[0]; y = v[1]; z = v[2];
715 }
716 #endif
717 
718 
719 #if INVENTOR
720 void Vect3::toSbVec3f(SbVec3f &vec) const
721 {
722 #if OPCOUNTS
723  vec.setValue(x.toDouble(), y.toDouble(), z.toDouble());
724 #else
725  vec.setValue(x, y, z);
726 #endif
727 }
728 #endif
729 
730 
731 int Vect3::operator==(const Vect3 &other) const
732 {
733  return x == other.x && y == other.y && z == other.z;
734 }
735 
736 
737 Real Vect3::dot(const Vect3 &other) const
738 {
739  return x * other.x + y * other.y + z * other.z;
740 }
741 
742 
743 Real Vect3::norm() const
744 {
745  return sqrt(x * x + y * y + z * z);
746 }
747 
748 
749 Real Vect3::norm2() const
750 {
751  return (x * x + y * y + z * z);
752 }
753 
754 
755 Real Vect3::distance(const Vect3 &other) const
756 {
757  Vect3 w;
758 
759  w.sub(other, *this);
760  return w.norm();
761 }
762 
763 
764 Real Vect3::distance2(const Vect3 &other) const
765 {
766  Vect3 w;
767 
768  w.sub(other, *this);
769  return w.norm2();
770 }
771 
772 
773 Real Vect3::min() const
774 {
775  return (x <= y) ? ((x <= z) ? x : z) : ((y <= z) ? y : z);
776 }
777 
778 
779 Real Vect3::max() const
780 {
781  return (x >= y) ? ((x >= z) ? x : z) : ((y >= z) ? y : z);
782 }
783 
784 
785 Real Vect3::minAbs() const
786 {
787  Real ax, ay, az;
788 
789  ax = fabs(x);
790  ay = fabs(y);
791  az = fabs(z);
792  return (ax <= ay) ? ((ax <= az) ? ax : az) : ((ay <= az) ? ay : az);
793 }
794 
795 
796 Real Vect3::maxAbs() const
797 {
798  Real ax, ay, az;
799 
800  ax = fabs(x);
801  ay = fabs(y);
802  az = fabs(z);
803  return (ax >= ay) ? ((ax >= az) ? ax : az) : ((ay >= az) ? ay : az);
804 }
805 
806 
807 void Vect3::swap(Vect3 &other)
808 {
809  Vect3 tmp;
810 
811  tmp = *this;
812  *this = other;
813  other = tmp;
814 }
815 
816 
817 void Vect3::normalize(const Vect3 &v)
818 {
819  Real s;
820 
821  s = 1.0 / sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
822  x = s * v.x;
823  y = s * v.y;
824  z = s * v.z;
825 }
826 
827 
828 void Vect3::normalize()
829 {
830  Real s;
831 
832  s = 1.0 / sqrt(x * x + y * y + z * z);
833  x *= s;
834  y *= s;
835  z *= s;
836 }
837 
838 
839 void Vect3::negate(const Vect3 &v)
840 {
841  x = - v.x;
842  y = - v.y;
843  z = - v.z;
844 }
845 
846 
847 void Vect3::negate()
848 {
849  x = - x;
850  y = - y;
851  z = - z;
852 }
853 
854 
855 void Vect3::add(const Vect3 &u, const Vect3 &v)
856 {
857  x = u.x + v.x;
858  y = u.y + v.y;
859  z = u.z + v.z;
860 }
861 
862 
863 void Vect3::add(const Vect3 &v)
864 {
865  x += v.x;
866  y += v.y;
867  z += v.z;
868 }
869 
870 
871 void Vect3::sub(const Vect3 &u, const Vect3 &v)
872 {
873  x = u.x - v.x;
874  y = u.y - v.y;
875  z = u.z - v.z;
876 }
877 
878 
879 void Vect3::sub(const Vect3 &v)
880 {
881  x -= v.x;
882  y -= v.y;
883  z -= v.z;
884 }
885 
886 
887 void Vect3::mult(const Vect3 &u, const Vect3 &v)
888 {
889  x = u.x * v.x;
890  y = u.y * v.y;
891  z = u.z * v.z;
892 }
893 
894 
895 void Vect3::mult(const Vect3 &v)
896 {
897  x *= v.x;
898  y *= v.y;
899  z *= v.z;
900 }
901 
902 
903 void Vect3::scale(const Vect3 &v, Real s)
904 {
905  x = s * v.x;
906  y = s * v.y;
907  z = s * v.z;
908 }
909 
910 
911 void Vect3::scale(Real s)
912 {
913  x *= s;
914  y *= s;
915  z *= s;
916 }
917 
918 
919 
920 void Vect3::cross(const Vect3 &u, const Vect3 &v)
921 {
922  x = u.y * v.z - u.z * v.y;
923  y = u.z * v.x - u.x * v.z;
924  z = u.x * v.y - u.y * v.x;
925 }
926 
927 
928 void Vect3::precross(const Vect3 &v)
929 {
930  Real ox, oy;
931 
932  ox = x;
933  oy = y;
934  x = v.y * z - v.z * oy;
935  y = v.z * ox - v.x * z;
936  z = v.x * oy - v.y * ox;
937 }
938 
939 
940 void Vect3::postcross(const Vect3 &v)
941 {
942  Real ox, oy;
943 
944  ox = x;
945  oy = y;
946  x = oy * v.z - z * v.y;
947  y = z * v.x - ox * v.z;
948  z = ox * v.y - oy * v.x;
949 }
950 
951 
952 void Vect3::crossAdd(const Vect3 &u, const Vect3 &v, const Vect3 &w)
953 {
954  x = u.y * v.z - u.z * v.y + w.x;
955  y = u.z * v.x - u.x * v.z + w.y;
956  z = u.x * v.y - u.y * v.x + w.z;
957 }
958 
959 
960 void Vect3::crossAdd(const Vect3 &u, const Vect3 &v)
961 {
962  x += u.y * v.z - u.z * v.y;
963  y += u.z * v.x - u.x * v.z;
964  z += u.x * v.y - u.y * v.x;
965 }
966 
967 
968 void Vect3::displace(const Vect3 &v, const Vect3 &u, Real lambda)
969 {
970  x = v.x + lambda * u.x;
971  y = v.y + lambda * u.y;
972  z = v.z + lambda * u.z;
973 }
974 
975 
976 void Vect3::displace(const Vect3 &u, Real lambda)
977 {
978  x += lambda * u.x;
979  y += lambda * u.y;
980  z += lambda * u.z;
981 }
982 
983 
984 void Vect3::interpolate(const Vect3 &u, const Vect3 &v, Real lambda)
985 {
986  Real lambda2 = 1.0 - lambda;
987 
988  x = lambda2 * u.x + lambda * v.x;
989  y = lambda2 * u.y + lambda * v.y;
990  z = lambda2 * u.z + lambda * v.z;
991 }
992 
993 
994 void Mat3::set(const Vect3 &diag, const Vect3 &sym)
995 {
996  xx = diag.x;
997  yy = diag.y;
998  zz = diag.z;
999  yz = zy = sym.x;
1000  zx = xz = sym.y;
1001  xy = yx = sym.z;
1002 }
1003 
1004 
1005 void Mat3::set(const Vect3 &axis, Real angle, int normalizeAxis)
1006 {
1007  Quat q;
1008 
1009  q.set(angle, axis, normalizeAxis);
1010  set(q);
1011 }
1012 
1013 void Mat3::setXcol(const Vect3 &v)
1014 {
1015  xx = v.x;
1016  yx = v.y;
1017  zx = v.z;
1018 }
1019 
1020 
1021 void Mat3::setYcol(const Vect3 &v)
1022 {
1023  xy = v.x;
1024  yy = v.y;
1025  zy = v.z;
1026 }
1027 
1028 
1029 void Mat3::setZcol(const Vect3 &v)
1030 {
1031  xz = v.x;
1032  yz = v.y;
1033  zz = v.z;
1034 }
1035 
1036 
1037 void Mat3::setSkew(const Vect3 &v)
1038 {
1039  xx = yy = zz = 0.0;
1040  zy = v.x;
1041  yz = -v.x;
1042  xz = v.y;
1043  zx = -v.y;
1044  yx = v.z;
1045  xy = -v.z;
1046 }
1047 
1048 
1049 Real Mat3::det() const
1050 {
1051  return xx * (yy * zz - yz * zy)
1052  + xy * (yz * zx - yx * zz)
1053  + xz * (yx * zy - yy * zx);
1054 }
1055 
1056 
1057 void Mat3::xpose(const Mat3 &M)
1058 {
1059  xx = M.xx;
1060  xy = M.yx;
1061  xz = M.zx;
1062 
1063  yx = M.xy;
1064  yy = M.yy;
1065  yz = M.zy;
1066 
1067  zx = M.xz;
1068  zy = M.yz;
1069  zz = M.zz;
1070 }
1071 
1072 
1073 void Mat3::xpose()
1074 {
1075  Real tmp;
1076 
1077  tmp = xy;
1078  xy = yx;
1079  yx = tmp;;
1080 
1081  tmp = yz;
1082  yz = zy;
1083  zy = tmp;
1084 
1085  tmp = zx;
1086  zx = xz;
1087  xz = tmp;
1088 }
1089 
1090 
1091 void Mat3::symmetrize(const Mat3 &M)
1092 {
1093  xx = 2 * M.xx;
1094  yy = 2 * M.yy;
1095  zz = 2 * M.zz;
1096  xy = yx = M.xy + M.yx;
1097  yz = zy = M.yz + M.zy;
1098  zx = xz = M.zx + M.xz;
1099 }
1100 
1101 
1102 void Mat3::symmetrize()
1103 {
1104  xx = 2 * xx;
1105  yy = 2 * yy;
1106  zz = 2 * zz;
1107  xy = yx = xy + yx;
1108  yz = zy = yz + zy;
1109  zx = xz = zx + xz;
1110 }
1111 
1112 
1113 void Mat3::negate(const Mat3 &M)
1114 {
1115  xx = - M.xx;
1116  xy = - M.xy;
1117  xz = - M.xz;
1118 
1119  yx = - M.yx;
1120  yy = - M.yy;
1121  yz = - M.yz;
1122 
1123  zx = - M.zx;
1124  zy = - M.zy;
1125  zz = - M.zz;
1126 }
1127 
1128 
1129 void Mat3::negate()
1130 {
1131  xx = - xx;
1132  xy = - xy;
1133  xz = - xz;
1134 
1135  yx = - yx;
1136  yy = - yy;
1137  yz = - yz;
1138 
1139  zx = - zx;
1140  zy = - zy;
1141  zz = - zz;
1142 }
1143 
1144 
1145 void Mat3::add(const Mat3 &M, const Mat3 &N)
1146 {
1147  xx = M.xx + N.xx;
1148  xy = M.xy + N.xy;
1149  xz = M.xz + N.xz;
1150 
1151  yx = M.yx + N.yx;
1152  yy = M.yy + N.yy;
1153  yz = M.yz + N.yz;
1154 
1155  zx = M.zx + N.zx;
1156  zy = M.zy + N.zy;
1157  zz = M.zz + N.zz;
1158 }
1159 
1160 
1161 void Mat3::add(const Mat3 &M)
1162 {
1163  xx += M.xx;
1164  xy += M.xy;
1165  xz += M.xz;
1166 
1167  yx += M.yx;
1168  yy += M.yy;
1169  yz += M.yz;
1170 
1171  zx += M.zx;
1172  zy += M.zy;
1173  zz += M.zz;
1174 }
1175 
1176 
1177 void Mat3::sub(const Mat3 &M, const Mat3 &N)
1178 {
1179  xx = M.xx - N.xx;
1180  xy = M.xy - N.xy;
1181  xz = M.xz - N.xz;
1182 
1183  yx = M.yx - N.yx;
1184  yy = M.yy - N.yy;
1185  yz = M.yz - N.yz;
1186 
1187  zx = M.zx - N.zx;
1188  zy = M.zy - N.zy;
1189  zz = M.zz - N.zz;
1190 }
1191 
1192 
1193 void Mat3::sub(const Mat3 &M)
1194 {
1195  xx -= M.xx;
1196  xy -= M.xy;
1197  xz -= M.xz;
1198 
1199  yx -= M.yx;
1200  yy -= M.yy;
1201  yz -= M.yz;
1202 
1203  zx -= M.zx;
1204  zy -= M.zy;
1205  zz -= M.zz;
1206 }
1207 
1208 
1209 void Mat3::scale(const Mat3 &M, Real s)
1210 {
1211  xx = s * M.xx;
1212  xy = s * M.xy;
1213  xz = s * M.xz;
1214  yx = s * M.yx;
1215  yy = s * M.yy;
1216  yz = s * M.yz;
1217  zx = s * M.zx;
1218  zy = s * M.zy;
1219  zz = s * M.zz;
1220 }
1221 
1222 
1223 void Mat3::scale(Real s)
1224 {
1225  xx *= s;
1226  xy *= s;
1227  xz *= s;
1228  yx *= s;
1229  yy *= s;
1230  yz *= s;
1231  zx *= s;
1232  zy *= s;
1233  zz *= s;
1234 }
1235 
1236 
1237 void Mat3::xform(const Vect3 &v, Vect3 &xv) const
1238 {
1239  xv.x = xx * v.x + xy * v.y + xz * v.z;
1240  xv.y = yx * v.x + yy * v.y + yz * v.z;
1241  xv.z = zx * v.x + zy * v.y + zz * v.z;
1242 }
1243 
1244 
1245 void Mat3::xform(Vect3 &v) const
1246 {
1247  Real ox, oy;
1248 
1249  ox = v.x; oy= v.y;
1250  v.x = xx * ox + xy * oy + xz * v.z;
1251  v.y = yx * ox + yy * oy + yz * v.z;
1252  v.z = zx * ox + zy * oy + zz * v.z;
1253 }
1254 
1255 
1256 void Mat3::invXform(const Vect3 &v, Vect3 &xv) const
1257 {
1258  xv.x = xx * v.x + yx * v.y + zx * v.z;
1259  xv.y = xy * v.x + yy * v.y + zy * v.z;
1260  xv.z = xz * v.x + yz * v.y + zz * v.z;
1261 }
1262 
1263 
1264 void Mat3::invXform(Vect3 &v) const
1265 {
1266  Real ox, oy;
1267 
1268  ox = v.x; oy= v.y;
1269  v.x = xx * ox + yx * oy + zx * v.z;
1270  v.y = xy * ox + yy * oy + zy * v.z;
1271  v.z = xz * ox + yz * oy + zz * v.z;
1272 }
1273 
1274 
1275 void MatX::set(const Se3 &T)
1276 {
1277  R.set(T.q);
1278  d = T.d;
1279 }
1280 
1281 ostream& MatX::print(ostream &os) const
1282 {
1283  return os << R << d << endl;
1284 }
1285 
1286 
1287 istream& MatX::read(istream &is)
1288 {
1289  Se3 T;
1290  is >> T;
1291  set(T);
1292  return is;
1293 }
1294 
1295 void MatX::xformVect(const Vect3 &v, Vect3 &xv) const
1296 {
1297  R.xform(v, xv);
1298 }
1299 
1300 
1301 void MatX::xformVect(Vect3 &v) const
1302 {
1303  R.xform(v);
1304 }
1305 
1306 
1307 void MatX::xformPoint(const Vect3 &p, Vect3 &xp) const
1308 {
1309  R.xform(p, xp);
1310  xp.add(d);
1311 }
1312 
1313 
1314 void MatX::xformPoint(Vect3 &p) const
1315 {
1316  R.xform(p);
1317  p.add(d);
1318 }
1319 
1320 
1321 void MatX::invXformVect(const Vect3 &v, Vect3 &xv) const
1322 {
1323  R.invXform(v, xv);
1324 }
1325 
1326 
1327 void MatX::invXformVect(Vect3 &v) const
1328 {
1329  R.invXform(v);
1330 }
1331 
1332 
1333 void MatX::invXformPoint(const Vect3 &p, Vect3 &xp) const
1334 {
1335  xp.sub(p, d);
1336  R.invXform(xp);
1337 }
1338 
1339 
1340 void MatX::invXformPoint(Vect3 &p) const
1341 {
1342  p.sub(d);
1343  R.invXform(p);
1344 }
1345 
1346 
1347 #if INVENTOR
1348 void Quat::set(const SbRotation &rot)
1349 {
1350  const float *q = rot.getValue();
1351  s_ = q[3]; x_ = q[0]; y_ = q[1]; z_ = q[2];
1352 }
1353 #endif
1354 
1355 
1356 #if INVENTOR
1357 void Quat::toSbRotation(SbRotation &rot) const
1358 {
1359 #if OPCOUNTS
1360  rot.setValue(x_.toDouble(), y_.toDouble(), z_.toDouble(), s_.toDouble());
1361 #else
1362  rot.setValue(x_, y_, z_, s_);
1363 #endif
1364 }
1365 #endif
1366 
1367 
1368 Vect3 Quat::axis() const
1369 {
1370  Vect3 v(x_, y_, z_);
1371  if (v.norm() == 0.0) v = Vect3::I; // axis is arbitrary here
1372  else v.normalize();
1373  return v;
1374 }
1375 
1376 
1377 Real Quat::angle() const
1378 {
1379 #if OPCOUNTS
1380  return 2 * acos(s_.toDouble());
1381 #else
1382  return 2 * acos(s_);
1383 #endif
1384 }
1385 
1386 
1387 void Quat::normalize(const Quat &q)
1388 {
1389  Real scale;
1390 
1391  scale = 1.0 / sqrt(q.s_*q.s_ + q.x_*q.x_ + q.y_*q.y_ + q.z_*q.z_);
1392  s_ = scale * q.s_;
1393  x_ = scale * q.x_;
1394  y_ = scale * q.y_;
1395  z_ = scale * q.z_;
1396 }
1397 
1398 
1399 void Quat::normalize()
1400 {
1401  Real scale;
1402 
1403  scale = 1.0 / sqrt(s_*s_ + x_*x_ + y_*y_ + z_*z_);
1404  s_ *= scale;
1405  x_ *= scale;
1406  y_ *= scale;
1407  z_ *= scale;
1408 }
1409 
1410 
1411 void Quat::invert(const Quat &q)
1412 {
1413  s_ = -q.s_;
1414  x_ = q.x_;
1415  y_ = q.y_;
1416  z_ = q.z_;
1417 }
1418 
1419 
1420 void Quat::invert()
1421 {
1422  s_ = -s_;
1423 }
1424 
1425 
1426 void Quat::deriv(const Quat &q, const Vect3 &w)
1427 {
1428  s_ = 0.5 * (-q.x_ * w.x - q.y_ * w.y - q.z_ * w.z);
1429  x_ = 0.5 * ( q.s_ * w.x - q.z_ * w.y + q.y_ * w.z);
1430  y_ = 0.5 * ( q.z_ * w.x + q.s_ * w.y - q.x_ * w.z);
1431  z_ = 0.5 * (-q.y_ * w.x + q.x_ * w.y + q.s_ * w.z);
1432 }
1433 
1434 
1435 #if INVENTOR
1436 void Se3::set(const SoTransform *xform)
1437 {
1438  const float *quat;
1439  const float *trans;
1440 
1441  quat = xform->rotation.getValue().getValue();
1442  q.x_ = quat[0]; q.y_ = quat[1]; q.z_ = quat[2]; q.s_ = quat[3];
1443 
1444  trans = xform->translation.getValue().getValue();
1445  d.x = trans[0];
1446  d.y = trans[1];
1447  d.z = trans[2];
1448 }
1449 #endif
1450 
1451 
1452 #if INVENTOR
1453 void Se3::toSoTransform(SoTransform *xform) const
1454 {
1455 #if OPCOUNTS
1456  xform->rotation.setValue
1457  (q.x_.toDouble(), q.y_.toDouble(), q.z_.toDouble(), q.s_.toDouble());
1458  xform->translation.setValue(d.x.toDouble(), d.y.toDouble(), d.z.toDouble());
1459 #else
1460  xform->rotation.setValue(q.x_, q.y_, q.z_, q.s_);
1461  xform->translation.setValue(d.x, d.y, d.z);
1462 #endif
1463 }
1464 #endif
1465 
1466 
1467 ostream& Se3::print(ostream &os) const
1468 {
1469  return os << q << d;
1470 }
1471 
1472 
1473 void Se3::mult(const Se3 &T, const Se3 &U)
1474 {
1475  q.mult(T.q, U.q);
1476  T.q.xform(U.d, d);
1477  d.add(d, T.d);
1478 }
1479 
1480 
1481 void Se3::premult(const Se3 &T)
1482 {
1483  q.premult(T.q);
1484  T.q.xform(d);
1485  d.add(T.d);
1486 }
1487 
1488 
1489 void Se3::postmult(const Se3 &T)
1490 {
1491  Vect3 v;
1492 
1493  q.xform(T.d, v);
1494  d.add(v);
1495  q.postmult(T.q);
1496 }
1497 
1498 
1499 void Se3::invert(const Se3 &T)
1500 {
1501  q.s_ = -T.q.s_;
1502  q.x_ = T.q.x_;
1503  q.y_ = T.q.y_;
1504  q.z_ = T.q.z_;
1505  q.xform(T.d, d);
1506  d.negate(d);
1507 }
1508 
1509 
1510 void Se3::invert()
1511 {
1512  q.s_ = -q.s_;
1513  q.xform(d);
1514  d.negate();
1515 }
1516 
1517 
1518 void Se3::xformVect(const Vect3 &v, Vect3 &xv) const
1519 {
1520  q.xform(v, xv);
1521 }
1522 
1523 
1524 void Se3::xformVect(Vect3 &v) const
1525 {
1526  q.xform(v);
1527 }
1528 
1529 
1530 void Se3::xformPoint(const Vect3 &p, Vect3 &xp) const
1531 {
1532  q.xform(p, xp);
1533  xp.add(d);
1534 }
1535 
1536 
1537 void Se3::xformPoint(Vect3 &p) const
1538 {
1539  q.xform(p);
1540  p.add(d);
1541 }
1542 
1543 
1544 void Se3::invXformVect(const Vect3 &v, Vect3 &xv) const
1545 {
1546  q.invXform(v, xv);
1547 }
1548 
1549 
1550 void Se3::invXformVect(Vect3 &v) const
1551 {
1552  q.invXform(v);
1553 }
1554 
1555 
1556 void Se3::invXformPoint(const Vect3 &p, Vect3 &xp) const
1557 {
1558  xp.sub(p, d);
1559  q.invXform(xp);
1560 }
1561 
1562 
1563 void Se3::invXformPoint(Vect3 &p) const
1564 {
1565  p.sub(d);
1566  q.invXform(p);
1567 }
1568 
1569 
1570 };
1571 #endif // #ifndef MV_H
1572 
1573 
Mat3(const Vect3 &axis, Real angle, int normalizeAxis=1)
Definition: mv.h:316
Vect3 & operator[](int i)
Definition: mv.h:333
static Real distance2(const Vect3 &u, const Vect3 &v)
Definition: mv.h:240
d
Real x_
Definition: mv.h:509
Real x
Definition: mv.h:190
Quat q
Definition: mv.h:606
double Real
Definition: mv.h:163
std::string normalize(std::string &str)
#define max(a, b)
Mat3(const Vect3 &diag, const Vect3 &sym)
Definition: mv.h:315
Se3(const MatX &X)
Definition: mv.h:616
static Real distance(const Vect3 &u, const Vect3 &v)
Definition: mv.h:239
Vect3 & trans()
Definition: mv.h:450
Real norm() const
Definition: mv.h:743
const Vect3 & operator[](int i) const
Definition: mv.h:332
Se3(const Quat &q_, const Vect3 &d_)
Definition: mv.h:615
Vect3()
Definition: mv.h:194
Real norm2() const
Definition: mv.h:749
Definition: defs.h:8
static const Vect3 K_
Definition: mv.h:285
int operator==(const PolyTreePair &ptree1, const PolyTreePair &ptree2)
Definition: vclip.h:252
static const Quat ID
Definition: mv.h:588
ostream & print(ostream &os) const
std::istream & operator>>(std::istream &is, std::vector< T > &v)
static void swap(Vect3 &u, Vect3 &v)
Definition: mv.h:241
Definition: mv.h:600
ostream & print(ostream &os) const
Definition: mv.h:1467
Real yy
Definition: mv.h:306
const Vect3 & xrow() const
Definition: mv.h:339
Real yx
Definition: mv.h:306
ostream & operator<<(ostream &os, const Se3 &T)
Definition: mv.h:695
Vect3 xcol() const
Definition: mv.h:348
Vect3 d
Definition: mv.h:607
ostream & print(ostream &os) const
Real distance2(const Vect3 &other) const
Definition: mv.h:764
Real z
Definition: mv.h:190
Vect3 sym() const
Definition: mv.h:358
int operator==(const Quat &other)
Definition: mv.h:548
Vect3 diag() const
Definition: mv.h:357
Mat3(const Quat &q)
Definition: mv.h:318
w
Real zx
Definition: mv.h:306
void swap(Vect3 &other)
Definition: mv.h:807
MatX(const Se3 &T)
Definition: mv.h:439
Mat3 R
Definition: mv.h:430
static const Mat3 ZERO
Definition: mv.h:411
png_infop png_bytep * trans
ROSCONSOLE_DECL void print(FilterBase *filter, void *logger, Level level, const char *file, int line, const char *function, const char *fmt,...) ROSCONSOLE_PRINTF_ATTRIBUTE(7
double det(const dmatrix &_a)
static const Mat3 ID
Definition: mv.h:412
static const Vect3 I
Definition: mv.h:280
Real z_
Definition: mv.h:509
Real z() const
Definition: mv.h:538
Vect3 & yrow()
Definition: mv.h:344
#define min(a, b)
static const Vect3 J
Definition: mv.h:281
Mat3()
Definition: mv.h:314
void normalize(const Vect3 &v)
Definition: mv.h:817
const Vect3 & trans() const
Definition: mv.h:628
Real y
Definition: mv.h:190
Quat()
Definition: mv.h:517
Real zy
Definition: mv.h:306
const Mat3 & rot() const
Definition: mv.h:447
istream & read(istream &is)
Real s_
Definition: mv.h:509
istream & read(istream &is)
Definition: mv.h:1287
Real zz
Definition: mv.h:306
Quat(Real s, Real x, Real y, Real z)
Definition: mv.h:518
ostream & print(ostream &os) const
static const Vect3 ZERO
Definition: mv.h:279
Mat3 & rot()
Definition: mv.h:449
Vect3 d
Definition: mv.h:431
Real xy
Definition: mv.h:306
ostream & print(ostream &os) const
Definition: mv.h:1281
Quat & rot()
Definition: mv.h:629
static Real dot(const Vect3 &u, const Vect3 &v)
Definition: mv.h:238
Vect3(Real x_, Real y_, Real z_)
Definition: mv.h:195
const Vect3 & yrow() const
Definition: mv.h:340
Quat(Real angle, const Vect3 &axis, int normalizeAxis=1)
Definition: mv.h:519
static const Vect3 J_
Definition: mv.h:284
Real y() const
Definition: mv.h:537
Real dot(const Vect3 &other) const
Definition: mv.h:737
Real y_
Definition: mv.h:509
const Vect3 & trans() const
Definition: mv.h:448
void xform(const Vect3 &u, Vect3 &v) const
Real x() const
Definition: mv.h:536
def norm(a)
Real s() const
Definition: mv.h:535
Quat(const Mat3 &R)
Definition: mv.h:521
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
const Vect3 & zrow() const
Definition: mv.h:341
double dot(const Vector3 &v1, const Vector3 &v2)
Definition: mv.h:54
Real xz
Definition: mv.h:306
MatX(const Mat3 &R_, const Vect3 &d_)
Definition: mv.h:438
static const Vect3 I_
Definition: mv.h:283
Real & operator[](int i)
Definition: mv.h:205
MatX()
Definition: mv.h:437
double norm2(const Vector3 &v)
const Quat & rot() const
Definition: mv.h:627
Vect3 zcol() const
Definition: mv.h:350
static const Se3 ID
Definition: mv.h:679
void set(Real s, Real x, Real y, Real z)
Definition: mv.h:525
Vect3 & xrow()
Definition: mv.h:343
Vect3 & zrow()
Definition: mv.h:345
Real xx
Definition: mv.h:306
Vect3 & trans()
Definition: mv.h:630
static const MatX ID
Definition: mv.h:489
const Real & operator[](int i) const
Definition: mv.h:204
void add(const Vect3 &u, const Vect3 &v)
Definition: mv.h:855
Se3()
Definition: mv.h:614
Vect3 ycol() const
Definition: mv.h:349
static const Vect3 K
Definition: mv.h:282
istream & read(istream &is)
Real yz
Definition: mv.h:306
Real distance(const Vect3 &other) const
Definition: mv.h:755
void sub(const Vect3 &u, const Vect3 &v)
Definition: mv.h:871


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20