source: Revenant/marslib/include/mars_linalg.h@ 25c4774

port/mars-tycoon
Last change on this file since 25c4774 was 88089e4, checked in by Jonathan Neufeld <support@…>, 3 years ago

Release Mars Linear Algebra library under the MIT license
  • Property mode set to 100755
File size: 40.6 KB
RevLine 
[88089e4]1/**
2 * This file, Mars Linag, is released under the MIT license
3 */
[80a6a52]4#ifndef __MARSLINEARALGEBRA_H__
5#define __MARSLINEARALGEBRA_H__
6
7#include <exception>
8#include <math.h>
9
10#ifdef _DEBUG
11#include <assert.h>
12#endif
13
14#include "mars_meta.h"
15#include "mars_calc.h"
16
17namespace mars
18{
19 class LinAlgEx : public std::exception
20 {
21 };
22
23 class MathOverrides
24 {
25 public:
26 static inline int sqrt (const int x)
27 { return static_cast <int> (::sqrt (static_cast <double> (x))); }
28 static inline short sqrt (const short x)
29 { return static_cast <short> (::sqrtf (static_cast <float> (x))); }
30 static inline long sqrt (const long x)
31 { return static_cast <long> (::sqrt (static_cast <long> (x))); }
32 static inline long double sqrt (const long double x)
33 { return static_cast <long double> (::sqrt (static_cast <long double> (x))); }
34 static inline double sqrt (const double x)
35 { return static_cast <double> (::sqrt (static_cast <double> (x))); }
36 static inline float sqrt (const float x)
37 { return static_cast <float> (::sqrtf (static_cast <float> (x))); }
38
39/*
40 static inline int atan2 (const int x, const int y)
41 { return static_cast <int> (::atan2 (static_cast <double> (x), static_cast <double> (y))); }
42 static inline short atan2 (const short x, const short y)
43 { return static_cast <short> (::atan2f (static_cast <float> (x), static_cast <float> (y))); }
44 static inline long double atan2 (const long double x, const long double y)
45 { return static_cast <long double> (::atan2 (static_cast <long double> (x), static_cast <long double> (y))); }
46 static inline double atan2 (const double x, const double y)
47 { return static_cast <double> (::atan2 (static_cast <double> (x), static_cast <double> (y))); }
48 static inline float atan2 (const float x, const float y)
49 { return static_cast <float> (::atan2f (static_cast <float> (x), static_cast <float> (y))); }
50
51 static inline int acos (const int x)
52 { return static_cast <int> (::acos (static_cast <double> (x))); }
53 static inline short acos (const short x)
54 { return static_cast <short> (::acosf (static_cast <float> (x))); }
55 static inline long double acos (const long double x)
56 { return static_cast <long double> (::acos (static_cast <long double> (x))); }
57 static inline double acos (const double x)
58 { return static_cast <double> (::acos (static_cast <double> (x))); }
59 static inline float acos (const float x)
60 { return static_cast <float> (::acosf (static_cast <float> (x))); }*/
61 };
62
63 // Linear optimization technique, taken from: http://www.flipcode.com/archives/Faster_Vector_Math_Using_Templates.shtml
64 // Operator controllers:
65
66 template <typename T>
67 struct VOpAdd
68 {
69 typedef T ResultType;
70
71 template <int I, typename LVal, typename RVal>
72 static inline T evalOp (const LVal & lval, const RVal & rval)
73 { return lval.template eval<I>() + rval.template eval<I>(); }
74 };
75 template <typename T>
76 struct VOpSub
77 {
78 typedef T ResultType;
79
80 template <int I, typename LVal, typename RVal>
81 static inline T evalOp (const LVal & lval, const RVal & rval)
82 { return lval.template eval<I>() - rval.template eval<I>(); }
83 };
84 template <typename T, int D>
85 struct VOpScale
86 {
87 typedef T ResultType;
88
89 template <int I, typename LVal, typename RVal>
90 static inline T evalOp (const LVal & lval, const RVal & rval)
91 { return lval.template eval<I>() * rval.template eval<I>(); }
92 };
93 template <typename T, int D>
94 struct VOpContract
95 {
96 typedef T ResultType;
97
98 template <int I, typename LVal, typename RVal>
99 static inline T evalOp (const LVal & lval, const RVal & rval)
100 { return lval.template eval<I>() / rval.template eval<I>(); }
101 };
102 template <typename T>
103 struct VOpNegate
104 {
105 typedef T ResultType;
106
107 template <int I, typename RVal>
108 static inline T evalOp (const RVal & rval)
109 { return -rval.template eval<I>(); }
110 };
111 template <typename T, typename TNew>
112 struct VOpCast
113 {
114 typedef TNew ResultType;
115
116 template <int I, typename RVal>
117 static inline TNew evalOp (const RVal & rval)
118 { return static_cast< TNew > (rval.template eval<I>()); }
119 };
120
121 template <typename T, int D>
122 struct VExprTagDim
123 {
124 typedef T Precision;
125 typedef VOpAdd< T > OpAdd;
126 typedef VOpSub< T > OpSub;
127 typedef VOpScale< T, D > OpScale;
128 typedef VOpContract< T, D > OpContract;
129 typedef VOpNegate< T > OpNegate;
130
131 enum { DIMENSION = D };
132 };
133
134 template <typename T, typename Derived, int D>
135 class VBaseExpr : public VExprTagDim< T, D >
136 {
137 public:
138 typedef Derived ExprType;
139
140 private:
141 template <typename Functor>
142 struct Reduce
143 {
144 template <unsigned I>
145 static inline const T go (const Functor & f, const Derived & expr) { return go(f, expr, quantity<I>()); }
146
147 template <unsigned I>
148 static inline const T go (const Derived & expr) { return go(expr, quantity<I>()); }
149
150 private:
151 template <unsigned I>
152 static inline const T go (const Functor & f, const Derived & expr, quantity<I>)
153 {
154 return f.run<I-1>(expr.template eval<I-1>()) + go<I-1>(f, expr);
155 }
156 static inline const T go (const Functor &, const Derived &, quantity<0>)
157 { return static_cast <T> (0); }
158
159 template <unsigned I>
160 static inline const T go (const Derived & expr, quantity<I>)
161 {
162 return Functor::template run<I-1>(expr.template eval<I-1>()) + go<I-1>(expr);
163 }
164 static inline const T go (const Derived &, quantity<0>)
165 { return static_cast <T> (0); }
166 };
167
168 template <typename Expr>
169 struct Equivalence
170 {
171 template <unsigned I>
172 static inline bool test (const Derived & lval, const Expr & rval) { return test(lval, rval, quantity<I>()); }
173
174 private:
175 template <unsigned I>
176 static inline bool test (const Derived & lval, const Expr & rval, quantity<I>)
177 {
178 return (lval.template eval<I-1>() == rval.template eval<I-1>()) && test<I-1>(lval, rval);
179 }
180 static inline bool test (const Derived &, const Expr &, quantity<0>) { return true; }
181 };
182
183 public:
184 template <typename Functor> inline const T reduce () const
185 { return Reduce<Functor>::template go<D>(*static_cast <const Derived *> (this)); }
186
187 template <typename Functor> inline const T reduce (const Functor & f) const
188 { return Reduce<Functor>::template go<D>(f, *static_cast <const Derived *> (this)); }
189
190 template <typename Expr>
191 inline bool equals (const Expr & expr, const typename Expr::ExprType * = 0) const
192 { return Equivalence< Expr >::template test<D>(*static_cast <const Derived *> (this), expr); }
193
194 template <typename V>
195 inline bool equals (const V & v, const typename V::VectorType * = 0) const
196 { return v == *static_cast <const Derived *> (this); }
197 };
198
199 struct VRedSqFn
200 {
201 template <int I, typename T>
202 static inline T run (const T & val) { return SQ(val); }
203 };
204
205 template <typename LVal>
206 class VRedDotEFn
207 {
208 private:
209 const typename LVal::ExprType _lval;
210
211 public:
212 inline VRedDotEFn (const LVal & lval)
213 : _lval(lval) {}
214
215 template <int I, typename T>
216 inline T run (const T & val) const { return val * _lval.template eval<I>(); }
217 };
218
219 template <typename V>
220 class VRedDotVFn
221 {
222 private:
223 const typename V::VectorType _v;
224
225 public:
226 inline VRedDotVFn (const V & v)
227 : _v(v) {}
228
229 template <int I, typename T>
230 inline T run (const T & val) const { return val * _v.template element<I>(); }
231 };
232
233 // Expression builders:
234 // Argument
235 template <typename T, int D>
236 class VExpArg : public VBaseExpr<T, VExpArg< T, D >, D >
237 {
238 private:
239 const T _arg;
240
241 public:
242 inline VExpArg (const T & argg)
243 : _arg (argg) {}
244
245 template <int I>
246 inline const T eval () const
247 { return _arg; }
248 };
249
250 // Vector
251 template <typename V, int D>
252 class VExpArgVec : public VBaseExpr< typename V::Precision, VExpArgVec< typename V::VectorType, D >, D >
253 {
254 private:
255 const V _v;
256
257 public:
258 inline VExpArgVec (const V & v)
259 : _v (v) {}
260
261 template <int I>
262 inline const typename V::Precision eval () const
263 { return _v.template element<I>(); }
264 };
265
266 // Pairing
267 template <class LVal, typename RVal, class Op, int D>
268 class VExpr2 : public VBaseExpr<typename Op::ResultType, VExpr2< typename LVal::ExprType, typename RVal::ExprType, Op, D >, D >
269 {
270 private:
271 const LVal _lval;
272 const RVal _rval;
273
274 public:
275 typedef VExpr2< typename LVal::ExprType, typename RVal::ExprType, Op, D > ExprType;
276 typedef VBaseExpr<typename Op::ResultType, VExpr2< typename LVal::ExprType, typename RVal::ExprType, Op, D >, D > Base;
277 typedef typename Op::ResultType Precision;
278
279 inline VExpr2 (const LVal & lval, const RVal & rval)
280 : _lval(lval), _rval(rval) {}
281
282 template <int I>
283 inline const typename Op::ResultType eval () const
284 { return Op::template evalOp<I>(_lval, _rval); }
285
286 public:
287 // Scalar
288 inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpScale< Precision, D >, D>
289 operator * (const Precision & n) const
290 { return VExpr2< ExprType, VExpArg< Precision, D >, VOpScale< Precision, D >, D> (*this, VExpArg< Precision, D > (n)); }
291
292 inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpContract< Precision, D >, D>
293 operator / (const Precision & n) const
294 { return VExpr2< ExprType, VExpArg< Precision, D >, VOpContract< Precision, D >, D> (*this, VExpArg< Precision, D > (n)); }
295
296 inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpAdd< Precision >, D>
297 operator + (const Precision & n) const
298 { return VExpr2< ExprType, VExpArg< Precision, D >, VOpAdd< Precision >, D> (*this, VExpArg< Precision, D > (n)); }
299
300 inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpSub< Precision >, D>
301 operator - (const Precision & n) const
302 { return VExpr2< ExprType, VExpArg< Precision, D >, VOpSub< Precision >, D> (*this, VExpArg< Precision, D > (n)); }
303
304 // Vector
305 template <typename V>
306 inline const VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpAdd< Precision >, D>
307 operator + (const V & v) const
308 { return VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpAdd< Precision >, D> (*this, VExpArgVec< V, D > (v)); }
309
310 template <typename V>
311 inline const VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpSub< Precision >, D>
312 operator - (const V & v) const
313 { return VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpSub< Precision >, D> (*this, VExpArgVec< V, D > (v)); }
314
315 // Other expressions
316 template <typename RVal2>
317 inline const VExpr2< ExprType, typename RVal2::ExprType, VOpAdd< Precision >, D>
318 operator + (const RVal2 & rval) const
319 { return VExpr2< ExprType, typename RVal2::ExprType, VOpAdd< Precision >, D> (*this, rval); }
320
321 template <typename RVal2>
322 inline const VExpr2< ExprType, typename RVal2::ExprType, VOpSub< Precision >, D>
323 operator - (const RVal2 & rval) const
324 { return VExpr2< ExprType, typename RVal2::ExprType, VOpSub< Precision >, D> (*this, rval); }
325
326 template <typename RVal2>
327 inline bool operator == (const RVal2 & rval) const { return equals(rval); }
328 };
329
330 // Mono
331 template <typename RVal, typename Op, int D>
332 class VExpr1 : public VBaseExpr<typename Op::ResultType, VExpr1< typename RVal::ExprType, Op, D >, D >
333 {
334 private:
335 const RVal _rval;
336
337 public:
338 typedef VExpr1< typename RVal::ExprType, Op, D > ExprType;
339 typedef typename Op::ResultType Precision;
340
341 inline VExpr1 (const typename RVal::ExprType & rval)
342 : _rval(rval) {}
343
344 template <int I>
345 inline const typename Op::ResultType eval () const
346 { return Op::template evalOp<I>(_rval); }
347
348 public:
349
350 // Scalar
351 inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpScale< Precision, D >, D>
352 operator * (const Precision & n) const
353 { return VExpr2< ExprType, VExpArg< Precision, D >, VOpScale< Precision, D >, D> (*this, VExpArg< Precision, D > (n)); }
354
355 inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpContract< Precision, D >, D>
356 operator / (const Precision & n) const
357 { return VExpr2< ExprType, VExpArg< Precision, D >, VOpContract< Precision, D >, D> (*this, VExpArg< Precision, D > (n)); }
358
359 inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpAdd< Precision >, D>
360 operator + (const Precision & n) const
361 { return VExpr2< ExprType, VExpArg< Precision, D >, VOpAdd< Precision >, D> (*this, VExpArg< Precision, D > (n)); }
362
363 inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpSub< Precision >, D>
364 operator - (const Precision & n) const
365 { return VExpr2< ExprType, VExpArg< Precision, D >, VOpSub< Precision >, D> (*this, VExpArg< Precision, D > (n)); }
366
367 // Vector
368 template <typename V>
369 inline const VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpAdd< Precision >, D>
370 operator + (const V & v) const
371 { return VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpAdd< Precision >, D> (*this, VExpArgVec< V, D > (v)); }
372
373 template <typename V>
374 inline const VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpSub< Precision >, D>
375 operator - (const V & v) const
376 { return VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpSub< Precision >, D> (*this, VExpArgVec< V, D > (v)); }
377
378 // Other expressions
379 template <typename RVal2>
380 inline const VExpr2< ExprType, typename RVal2::ExprType, VOpAdd< Precision >, D>
381 operator + (const RVal2 & rval) const
382 { return VExpr2< ExprType, typename RVal2::ExprType, VOpAdd< Precision >, D> (*this, rval); }
383
384 template <typename RVal2>
385 inline const VExpr2< ExprType, typename RVal2::ExprType, VOpSub< Precision >, D>
386 operator - (const RVal2 & rval) const
387 { return VExpr2< ExprType, typename RVal2::ExprType, VOpSub< Precision >, D> (*this, rval); }
388
389 template <typename RVal2>
390 inline bool operator == (const RVal2 & rval) const { return equals(rval); }
391 };
392
393 template <typename T, typename V, typename Desc, int D >
394 struct VectorDefsTag : public VExprTagDim< T, D >
395 {
396 typedef VectorDefsTag<T , V, Desc, D> VectorTagType;
397 typedef V VectorType;
398 typedef VExpArg< T, D > Arg;
399 typedef VExpArgVec< V, D > ArgVec;
400
401 template <int I>
402 inline const T element () const
403 {
404#ifdef _DEBUG
405 int i = I;
406 assert(I < D);
407#endif
408 return reinterpret_cast <const T *> (static_cast <const Desc *> (this)) [I];
409 }
410 };
411
412 template <typename T, typename V, typename Desc, int D >
413 class ArithmeticVector : public VectorDefsTag< T, V, Desc, D >
414 {
415 public:
416 //typedef ArithmeticVector< T, V, Desc, D > ArithmeticVectorType;
417 typedef VectorDefsTag< T, V, Desc, D > Base;
418 typedef VExprTagDim< T, D > ExprTag;
419
420 using typename Base::Arg;
421 using typename Base::ArgVec;
422 using typename ExprTag::OpNegate;
423 using typename ExprTag::OpScale;
424 using typename ExprTag::OpContract;
425 using typename ExprTag::OpAdd;
426 using typename ExprTag::OpSub;
427
428 typedef VExpr1< ArgVec, OpNegate, D > ExpNeg;
429 typedef VExpr2< ArgVec, Arg, OpScale, D > ExpScale;
430 typedef VExpr2< ArgVec, Arg, OpContract, D > ExpContract;
431 typedef VExpr2< ArgVec, Arg, OpAdd, D > ExpAddScalar;
432 typedef VExpr2< ArgVec, ArgVec, OpAdd, D > ExpAddVector;
433 typedef VExpr2< ArgVec, Arg, OpSub, D > ExpSubScalar;
434 typedef VExpr2< ArgVec, ArgVec, OpSub, D > ExpSubVector;
435
436 template <typename Expr>
437 struct ExpOpExp
438 {
439 typedef VExpr2< ArgVec, typename Expr::ExprType, OpAdd, D > Add;
440 typedef VExpr2< ArgVec, typename Expr::ExprType, OpSub, D > Sub;
441 };
442
443 // Negation
444 inline ExpNeg neg () const
445 { return ExpNeg (ArgVec(* static_cast <const V *> (this))); }
446
447 // Scaling
448 inline ExpScale scale (const T & n) const
449 { return ExpScale (ArgVec(* static_cast <const V *> (this)), Arg(n)); }
450
451 // Contraction
452 inline ExpContract contract (const T & n) const
453 { return ExpContract (ArgVec(* static_cast <const V *> (this)), Arg(n)); }
454
455 // Add scalar
456 inline ExpAddScalar add (const T & n) const
457 { return ExpAddScalar (ArgVec(* static_cast <const V *> (this)), Arg(n)); }
458
459 // Add vector
460 inline ExpAddVector add (const V & v) const
461 { return ExpAddVector (ArgVec(* static_cast <const V *> (this)), ArgVec(v)); }
462
463 // Subtract scalar
464 inline ExpSubScalar sub (const T & n) const
465 { return ExpSubScalar (ArgVec(* static_cast <const V *> (this)), Arg(n)); }
466
467 // Subtract vector
468 inline ExpSubVector sub (const V & v) const
469 { return ExpSubVector (ArgVec(* static_cast <const V *> (this)), ArgVec(v)); }
470 };
471
472 template <typename T, typename V>
473 struct DescV2 : public ArithmeticVector< T, V, DescV2< T, V >, 2 >
474 {
475 typedef ArithmeticVector< T, V, DescV2< T, V >, 2 > Base;
476
477 T x, y;
478
479 inline DescV2 ()
480 : x(0), y(0) {}
481 inline DescV2 (const T x, const T y)
482 : x(x), y(y) {}
483
484 template <typename E, typename Vv>
485 inline DescV2 (const DescV2 <E, Vv> & copy)
486 : x(static_cast <T> (copy.x)),
487 y(static_cast <T> (copy.y)) {}
488
489 template <typename Exp>
490 inline V & operator = (const Exp & expr) const
491 {
492 x = expr.template eval<0>();
493 y = expr.template eval<1>();
494 return * static_cast <const V *> (this);
495 }
496 };
497
498 template <typename T, typename V>
499 struct DescV3 : public ArithmeticVector< T, V, DescV3< T, V >, 3 >
500 {
501 typedef ArithmeticVector< T, V, DescV3< T, V >, 3 > Base;
502
503 T x, y, z;
504
505 inline DescV3 ()
506 : x(0), y(0), z(0) {}
507 inline DescV3 (const T x, const T y, const T z)
508 : x(x), y(y), z(z) {}
509
510 template <typename E, typename Vv>
511 inline DescV3 (const DescV3 <E, Vv> & copy)
512 : x(static_cast <T> (copy.x)),
513 y(static_cast <T> (copy.y)),
514 z(static_cast <T> (copy.z)) {}
515
516 template <typename Exp>
517 inline V & operator = (const Exp & expr) const
518 {
519 x = expr.template eval<0>();
520 y = expr.template eval<1>();
521 z = expr.template eval<2>();
522 return * static_cast <const V *> (this);
523 }
524 };
525
526 template <typename T, typename V>
527 struct DescQ : public VectorDefsTag< T, V, DescQ< T, V >, 3 >
528 {
529 typedef VectorDefsTag< T, V, DescQ< T, V >, 3 > Base;
530
531 T x, y, z, w;
532
533 inline DescQ ()
534 : x(0), y(0), z(0), w(0) {}
535 inline DescQ (const T x, const T y, const T z, const T w)
536 : x(x), y(y), z(z), w(w) {}
537
538 template <typename Vv>
539 inline DescQ (const DescV3< T, Vv> & v)
540 : x(v.x), y(v.y), z(v.z), w(0) {}
541
542 template <typename Vv>
543 inline DescQ (const T s, const DescV3< T, Vv> & v)
544 : x(v.x), y(v.y), z(v.z), w(s) {}
545
546 template <typename E>
547 inline DescQ (const DescQ <E, V> & copy)
548 : x(static_cast <T> (copy.x)),
549 y(static_cast <T> (copy.y)),
550 z(static_cast <T> (copy.z)),
551 w(static_cast <T> (copy.w)) {}
552 };
553
554 template <typename T> class vector3D;
555 template <typename T> class SphericalCoords;
556 template <typename T> class PolarCoords;
557 template <typename T> class CylindricalCoords;
558
559 // *** Vector 2D class template ***
560 template <typename T>
561 class vector2D : public DescV2< T, vector2D< T > >, private MathOverrides
562 {
563 private:
564 typedef DescV2< T, vector2D< T > > Base;
565
566 public:
567 template <typename Exp>
568 inline vector2D (const Exp & expr, const typename Exp::ExprType * dummy = 0)
569 : DescV2< T, vector2D< T > >(expr.template eval<0>(), expr.template eval<1>()) {}
570
571 inline vector2D ()
572 : DescV2< T, vector2D< T > >() {}
573 inline vector2D (const T x, const T y)
574 : DescV2< T, vector2D< T > >(x, y) {}
575 template <typename E>
576 inline vector2D (const vector2D <E> & other)
577 : DescV2< T, vector2D< T > >(other) {}
578
579 // BEGIN ArithmeticVector INHERITANCE {
580 // Binary expression mappings
581 template <typename Expr>
582 inline typename Base::template ExpOpExp<typename Expr::ExprType>::Add operator + (const Expr & rval) const
583 { return typename Base::template ExpOpExp<typename Expr::ExprType>::Add (typename Base::ArgVec(*this), rval); }
584
585 template <typename Expr>
586 inline typename Base::template ExpOpExp<typename Expr::ExprType>::Sub operator - (const Expr & rval) const
587 { return typename Base::template ExpOpExp<typename Expr::ExprType>::Sub (typename Base::ArgVec(*this), rval); }
588
589 template <typename Expr>
590 inline typename enable_ifb< has_def< typename Expr::ExprType >::value, bool >::Type operator == (const Expr & rval) const
591 { return rval.equals(*this); }
592
593 // Import dependent types
594 using typename Base::ExpNeg;
595 using typename Base::ExpScale;
596 using typename Base::ExpContract;
597 using typename Base::ExpAddScalar;
598 using typename Base::ExpAddVector;
599 using typename Base::ExpSubScalar;
600 using typename Base::ExpSubVector;
601
602 using typename Base::VectorType;
603
604 // Basic binary mappings
605 inline ExpNeg operator - () const { return this->neg(); }
606 inline ExpScale operator * (const T & n) const { return this->scale(n); }
607 inline ExpContract operator / (const T & n) const { return this->contract(n); }
608 inline ExpAddScalar operator + (const T & n) const { return this->add(n); }
609 inline ExpAddVector operator + (const VectorType & v) const { return this->add(v); }
610 inline ExpSubScalar operator - (const T & n) const { return this->sub(n); }
611 inline ExpSubVector operator - (const VectorType & v) const { return this->sub(v); }
612 // } END ArithmeticVector INHERITANCE
613
614 // Dot-product
615 inline T operator * (const vector2D<T> & v) const
616 { return this->x * v.x + this->y * v.y; }
617
618 // Maps to 2D-cross product below
619 inline const vector2D <T> operator & (const vector3D <T> & v) const
620 { return operator & (v.z); }
621
622 // 2D-cross product (perpendicular)
623 inline const vector2D <T> operator & (const T s) const
624 { return vector2D<T> (this->y * s, -this->x * s); }
625
626 // Accumulate scalar
627 inline const vector2D <T> & operator += (const T & n)
628 {
629 this->x += n;
630 this->y += n;
631 return *this;
632 }
633
634 // Accumulate vector
635 inline const vector2D <T> & operator += (const vector2D<T> & v)
636 {
637 this->x += v.x;
638 this->y += v.y;
639
640 return *this;
641 }
642
643 // Decrement vector
644 inline const vector2D <T> & operator -= (const vector2D<T> & v)
645 {
646 this->x -= v.x;
647 this->y -= v.y;
648
649 return *this;
650 }
651 inline T magSQ () const { return static_cast <T> (SQ(this->x) + SQ(this->y)); }
652 inline T magnitude () const { return static_cast <T> (sqrt(magSQ())); }
653 inline vector2D <T> normalize () const
654 {
655 const T m = magnitude();
656
657 if (m == 0)
658 return vector2D <T> (0,0);
659 else
660 return vector2D<T> (
661 this->x / m,
662 this->y / m
663 );
664 }
665
666 template <typename J>
667 inline bool operator != (const vector2D <J> & other)
668 {
669 return this->x != other.x || this->y != other.y;
670 }
671 template <typename J>
672 inline bool operator == (const vector2D <J> & other)
673 {
674 return this->x == other.x && this->y == other.y;
675 }
676
677 // Assignment
678 template <typename J>
679 inline vector2D <T> & operator = (const vector2D <J> & copy)
680 {
681 this->x = static_cast <T> (copy.x);
682 this->y = static_cast <T> (copy.y);
683
684 return *this;
685 }
686
687 // Scale vector
688 inline vector2D <T> & operator *= (const T factor)
689 {
690 this->x *= factor;
691 this->y *= factor;
692 return *this;
693 }
694
695 // Contract vector
696 inline vector3D <T> & operator /= (const T factor)
697 {
698 this->x /= factor;
699 this->y /= factor;
700 return *this;
701 }
702
703 // Length comparisons
704 inline bool operator < (const T len) const
705 { return magSQ() < SQ(len); }
706 inline bool operator > (const T len) const
707 { return magSQ() > SQ(len); }
708 inline bool operator <= (const T len) const
709 { return magSQ() <= SQ(len); }
710 inline bool operator >= (const T len) const
711 { return magSQ() >= SQ(len); }
712
713 inline bool operator == (const vector2D< T > & b) const
714 { return this->x == b.x && this->y == b.y; }
715 };
716
717 // *** Vector 3D class template ***
718 template <typename T>
719 class vector3D : public DescV3< T, vector3D< T > >, private MathOverrides
720 {
721 private:
722 typedef DescV3< T, vector3D< T > > Base;
723
724 public:
725 template <typename Exp>
726 inline vector3D (const Exp & expr, const typename Exp::ExprType * dummy = 0)
727 : DescV3< T, vector3D< T > >(expr.template eval<0>(), expr.template eval<1>(), expr.template eval<2>()) {}
728
729 template <typename E>
730 inline vector3D (const SphericalCoords< E > & sphc)
731 : DescV3< T, vector3D< T > >(
732 static_cast <T> (sphc.r * sin(sphc.zenith) * cos(sphc.azimuth)),
733 static_cast <T> (sphc.r * sin(sphc.zenith) * sin(sphc.azimuth)),
734 static_cast <T> (sphc.r * cos(sphc.zenith))
735 ) {}
736
737 inline vector3D ()
738 : DescV3< T, vector3D< T > >() {}
739 inline vector3D (const T x, const T y, const T z)
740 : DescV3< T, vector3D< T > >(x, y, z) {}
741
742 template <typename E>
743 inline vector3D (const vector3D <E> & other)
744 : DescV3< T, vector3D< T > > (other) {}
745
746 // Spherical adapter
747 inline vector3D operator = (const SphericalCoords <T> & sphc)
748 {
749 this->x = sphc.r * sin(sphc.zenith) * cos(sphc.azimuth);
750 this->y = sphc.r * sin(sphc.zenith) * sin(sphc.azimuth);
751 this->z = sphc.r * cos(sphc.zenith);
752 }
753 template <typename E>
754 inline vector3D<E> & cast ()
755 {
756 return vector3D <E> (
757 static_cast<T> (this->other.x),
758 static_cast<T> (this->other.y),
759 static_cast<T> (this->other.z)
760 );
761 }
762
763 // BEGIN ArithmeticVector INHERITANCE {
764 // Binary expression mappings
765 template <typename Expr>
766 inline typename Base::template ExpOpExp<typename Expr::ExprType>::Add operator + (const Expr & rval) const
767 { return typename Base::template ExpOpExp<typename Expr::ExprType>::Add (typename Base::ArgVec(*this), rval); }
768
769 template <typename Expr>
770 inline typename Base::template ExpOpExp<typename Expr::ExprType>::Sub operator - (const Expr & rval) const
771 { return typename Base::template ExpOpExp<typename Expr::ExprType>::Sub (typename Base::ArgVec(*this), rval); }
772
773 template <typename Expr>
774 inline typename enable_ifb< has_def< typename Expr::ExprType >::value, bool >::Type operator == (const Expr & rval) const
775 { return rval.equals(*this); }
776
777 // Import dependent types
778 using typename Base::ExpNeg;
779 using typename Base::ExpScale;
780 using typename Base::ExpContract;
781 using typename Base::ExpAddScalar;
782 using typename Base::ExpAddVector;
783 using typename Base::ExpSubScalar;
784 using typename Base::ExpSubVector;
785
786 using typename Base::VectorType;
787
788 // Basic binary mappings
789 inline ExpNeg operator - () const { return this->neg(); }
790 inline ExpScale operator * (const T & n) const { return this->scale(n); }
791 inline ExpContract operator / (const T & n) const { return this->contract(n); }
792 inline ExpAddScalar operator + (const T & n) const { return this->add(n); }
793 inline ExpAddVector operator + (const VectorType & v) const { return this->add(v); }
794 inline ExpSubScalar operator - (const T & n) const { return this->sub(n); }
795 inline ExpSubVector operator - (const VectorType & v) const { return this->sub(v); }
796 // } END ArithmeticVector INHERITANCE
797
798 // Shortcut mappings
799 inline T operator * (const vector3D<T> & v) const { return dot(v); }
800 inline bool operator || (const vector3D<T> & v) const { return orthogonal(v); }
801 inline const vector3D <T> operator & (const vector3D <T> & v) const { return cross(v); }
802
803 inline bool operator == (const vector3D< T > & b) const { return equals(b); }
804 template <typename J> inline bool operator != (const vector3D <J> & other) const { return nequal(other); }
805 template <typename J> inline bool operator == (const vector3D <J> & other) const { return equals(other); }
806
807 inline bool operator < (const T len) const { return lesser(len); }
808 inline bool operator > (const T len) const { return greater(len); }
809 inline bool operator <= (const T len) const { return lessequal(len); }
810 inline bool operator >= (const T len) const { return greatequal(len); }
811
812 // Vector2 adapter
813 inline operator vector2D <T> () const { return vector2D <T> (this->x, this->y); }
814
815 // 3D-specific operations
816 inline T dot (const vector3D<T> & v) const
817 { return this->x * v.x + this->y * v.y + this->z * v.z; }
818 inline bool orthogonal (const vector3D<T> & v) const
819 { return Matrix3D(vector3D<T>(1,1,1), *this, v) == 0; }
820 inline const vector3D <T> cross (const vector3D <T> & v) const
821 {
822 return vector3D <T> (
823 this->y*v.z - this->z*v.y,
824 this->z*v.x - this->x*v.z,
825 this->x*v.y - this->y*v.x
826 );
827 }
828
829 // Length
830 inline T magSQ () const { return static_cast <T> (SQ(this->x) + SQ(this->y) + SQ(this->z)); }
831 inline T magnitude () const { return static_cast <T> (sqrt(magSQ())); }
832 inline vector3D <T> normalize () const
833 {
834 const T m = magnitude();
835
836 if (m == 0)
837 return vector3D <T> (0,0,0);
838 else
839 return vector3D<T> (
840 this->x / m,
841 this->y / m,
842 this->z / m
843 );
844 }
845
846 // Equivalence
847 inline bool equals (const vector3D< T > & b) const
848 { return this->x == b.x && this->y == b.y && this->z == b.z; }
849
850 template <typename J>
851 inline bool nequal (const vector3D <J> & other) const
852 { return this->x != other.x || this->y != other.y || this->z != other.z; }
853
854 template <typename J>
855 inline bool equals (const vector3D <J> & other) const
856 { return this->x == other.x && this->y == other.y && this->z == other.z; }
857
858 // Length comparisons
859 inline bool lesser (const T len) const
860 { return magSQ() < SQ(len); }
861 inline bool greater (const T len) const
862 { return magSQ() > SQ(len); }
863 inline bool lessequal (const T len) const
864 { return magSQ() <= SQ(len); }
865 inline bool greatequal (const T len) const
866 { return magSQ() >= SQ(len); }
867
868 inline vector2D <T> v2D() const { return vector2D <T> (this->x, this->y); }
869
870 // Accumulate scalar
871 inline const vector3D <T> & operator += (const T & n)
872 {
873 this->x += n;
874 this->y += n;
875 this->z += n;
876 }
877
878 // Accumulate vector
879 inline const vector3D <T> & operator += (const vector3D<T> & v)
880 {
881 this->x += v.x;
882 this->y += v.y;
883 this->z += v.z;
884
885 return *this;
886 }
887
888 // Decrement by vector
889 inline const vector3D <T> & operator -= (const vector3D<T> & v)
890 {
891 this->x -= v.x;
892 this->y -= v.y;
893 this->z -= v.z;
894
895 return *this;
896 }
897
898 // Assignment
899 template <typename J>
900 inline vector3D <T> & operator = (const vector3D <J> & copy)
901 {
902 this->x = static_cast <T> (copy.x);
903 this->y = static_cast <T> (copy.y);
904 this->z = static_cast <T> (copy.z);
905
906 return *this;
907 }
908
909 // Scale vector
910 inline vector3D <T> & operator *= (const T factor)
911 {
912 this->x *= factor;
913 this->y *= factor;
914 this->z *= factor;
915 return *this;
916 }
917
918 // Contract vector
919 inline vector3D <T> & operator /= (const T factor)
920 {
921 this->x /= factor;
922 this->y /= factor;
923 this->z /= factor;
924 return *this;
925 }
926 };
927 // Swap-operator mappings
928 template <typename V>
929 inline typename V::ExpScale operator * (const typename V::Precision f, const V & v)
930 { return v.scale(f); }
931
932 template <typename T, typename RVal>
933 inline VExpr2< typename RVal::ExprType, VExpArg< T, RVal::DIMENSION >, VOpScale< T, RVal::DIMENSION >, RVal::DIMENSION >
934 operator * (const T f, const RVal & rval)
935 { return VExpr2< typename RVal::ExprType, VExpArg< T, RVal::DIMENSION >, VOpScale< T, RVal::DIMENSION >, RVal::DIMENSION > (rval, VExpArg< T, RVal::DIMENSION > (f)); }
936
937 // *** Generic mappings ***
938 // ** Expressions **
939 template <typename E>
940 inline typename E::ExprType::Precision
941 MAG(const E & expr) { return MathOverrides::sqrt(expr.template reduce<VRedSqFn>()); }
942
943 template <typename E>
944 inline typename E::ExprType::Precision
945 MAGSQ(const E & expr) { return expr.template reduce<VRedSqFn>(); }
946
947 template <typename V>
948 inline typename V::VectorType::Precision
949 DOT(const V & a, const V & b) { return a * b; }
950
951 template <typename V, typename Expr>
952 inline typename enable_ifb< andb< has_def< typename V::VectorType>::value, has_def< typename Expr::ExprType >::value >::value, typename V::Precision >::Type
953 DOT(const V & a, const Expr & b) { return b.template reduce< VRedDotVFn< V > > (VRedDotVFn< V >(a)); }
954
955 template <typename Expr, typename V>
956 inline typename enable_ifb< andb< has_def< typename Expr::ExprType >::value, has_def< typename V::VectorType >::value >::value, typename Expr::Precision >::Type
957 DOT(const Expr & a, const V & b) { return a.template reduce< VRedDotVFn< V > > (VRedDotVFn< V >(b)); }
958
959 template <typename LVal, typename RVal>
960 inline typename enable_ifb< andb< has_def< typename LVal::ExprType >::value, has_def< typename RVal::ExprType >::value >::value, typename LVal::Precision >::Type
961 DOT(const LVal & a, const RVal & b) { return a.template reduce<VRedDotEFn< RVal > > (VRedDotEFn< RVal > (b)); }
962
963 template <typename A, typename B>
964 inline typename enable_ifb< andb< has_def<typename A::VExprTagType>::value, has_def<typename B::VExprTagType>::value >::value, typename A::Precision>::Type
965 ANGLE (const A & a, const B & b) { return acosf (DOT(a,b) / (MAG(a) * MAG(b))); }
966
967 template <typename V>
968 inline typename V::VectorType::Precision
969 MAG(const V & v) { return v.magnitude(); }
970
971 template <typename V>
972 inline typename V::VectorType::Precision
973 MAGSQ(const V & v) { return v.magSQ(); }
974
975 template <typename V>
976 inline typename V::VectorType
977 U(const V & a) { return a.normalize(); }
978
979 template <typename Expr, typename V>
980 inline typename enable_ifb< andb< has_def< typename Expr::ExprType >::value, has_def< typename V::VectorType >::value >::value >::Type
981 U(const Expr & a) { return V(a.normalize()); }
982
983 template < typename TNew, typename E >
984 inline VExpr1< E, VOpCast< typename E::ExprType::Precision, TNew >, E::ExprType::DIMENSION >
985 CAST(const E & expr) { return VExpr1< E, VOpCast< typename E::ExprType::Precision, TNew >, E::ExprType::DIMENSION > (expr); }
986
987 typedef vector2D<float> vector2Df;
988 typedef vector2D<double> vector2Dd;
989 typedef vector3D<float> vector3Df;
990 typedef vector3D<double> vector3Dd;
991
992 // Spherical coordinates, definition taken from http://mathworld.wolfram.com/SphericalCoordinates.html
993 template <typename T>
994 class SphericalCoords : private MathOverrides
995 {
996 public:
997 typedef T Precision;
998
999 T r;
1000 float
1001 zenith, // The circle with a vertical bar through it, thus represents the polar angle
1002 azimuth; // Looks like "theta", circle with horizontal bar through it, thus represents the angle in the x/y plane
1003
1004 inline SphericalCoords () : r(0), zenith(0), azimuth(0) {}
1005
1006 inline SphericalCoords (const vector3D <T> & v)
1007 : r(static_cast< T > (sqrt(SQ(v.x) + SQ(v.y) + SQ(v.z)))),
1008 zenith(atan2f(static_cast< float > (v.y), static_cast< float > (v.x))),
1009 azimuth(0)
1010 {
1011 if (r != 0)
1012 azimuth = acos(static_cast< float > (v.z) / static_cast< float > (r));
1013 }
1014 inline SphericalCoords (const T r, const float zenith, const float azimuth)
1015 : r(r), zenith(zenith), azimuth(azimuth) {}
1016
1017 inline SphericalCoords <T> & operator = (const vector3D <T> & v)
1018 {
1019 r = sqrt(SQ(v.x) + SQ(v.y) + SQ(v.z));
1020
1021 azimuth = atan2(
1022 static_cast< float > (v.y),
1023 static_cast< float > (v.x)
1024 );
1025
1026 if (r != 0)
1027 zenith = acos(
1028 static_cast< float > (v.z)
1029 /
1030 static_cast< float > (r)
1031 );
1032
1033 return *this;
1034 }
1035 inline const vector3D <T> operator + (const vector3D <T> rval) const { return static_cast <const vector3D<T> > (*this) + rval; }
1036 inline operator const vector3D <T> () const
1037 {
1038 return vector3D <T>
1039 (
1040 static_cast< T > (static_cast< float > (r) * sin(zenith) * cos(azimuth)),
1041 static_cast< T > (static_cast< float > (r) * sin(zenith) * sin(azimuth)),
1042 static_cast< T > (static_cast< float > (r) * cos(zenith))
1043 );
1044 }
1045 inline SphericalCoords <T> & operator = (const CylindricalCoords <T> & c)
1046 {
1047 r = static_cast< T > (sqrt(SQ(c.p) + SQ(c.z)));
1048 zenith = atan2(c.p, c.z);
1049 azimuth = c.azimuth;
1050 return *this;
1051 }
1052 inline operator const CylindricalCoords <T> () const
1053 {
1054 return CylindricalCoords <T>
1055 (
1056 static_cast< T > (static_cast< float > (r) * sin(zenith)),
1057 azimuth,
1058 static_cast< T > (static_cast< float > (r) * cos(zenith))
1059 );
1060 }
1061 };
1062
1063 template <typename T>
1064 class PolarCoords
1065 {
1066 private:
1067 inline PolarCoords <T> & assign (const T x, const T y)
1068 {
1069 p = sqrt(SQ(x) + SQ(y));
1070 azimuth = atan2(y, x);
1071 return *this;
1072 }
1073
1074 public:
1075 T p;
1076 float azimuth;
1077
1078 inline PolarCoords () : p(0), azimuth(0) {}
1079 inline PolarCoords (const vector2D <T> & v) : p(sqrt(SQ(v.x)) + sqrt(SQ(v.y))), azimuth(atan2(v.y, v.x)) {}
1080
1081 inline PolarCoords (const T p, const float azimuth)
1082 : p(p), azimuth(azimuth) {}
1083
1084 inline PolarCoords <T> & operator = (const vector2D <T> & v)
1085 { return assign(v.x, v.y); }
1086 inline PolarCoords <T> & operator = (const vector3D <T> & v)
1087 { return assign(v.x, v.y); }
1088
1089 template <typename J>
1090 inline operator const vector2D <J> () const
1091 {
1092 return vector2D <J> (
1093 static_cast <J> (static_cast <double> (p) * cos(azimuth)),
1094 static_cast <J> (static_cast <double> (p) * sin(azimuth))
1095 );
1096 }
1097 inline PolarCoords <T> & operator = (const SphericalCoords <T> & s)
1098 {
1099 p = s.r * sin(s.zenith);
1100 azimuth = s.azimuth;
1101 return *this;
1102 }
1103 };
1104
1105 template <typename T>
1106 class CylindricalCoords : public PolarCoords <T>
1107 {
1108 public:
1109 T z;
1110
1111 inline CylindricalCoords () : PolarCoords<T> (0, 0), z(0) {}
1112 inline CylindricalCoords (const vector3D <T> & v) : PolarCoords<T> (v), z(v.z) {}
1113
1114 inline CylindricalCoords (const T p, const float azimuth, const T z)
1115 : PolarCoords<T>(p, azimuth), z(z) {}
1116
1117 inline CylindricalCoords <T> & operator = (const vector3D <T> & v)
1118 {
1119 PolarCoords <T>::operator = (v);
1120 z = v.z;
1121 return *this;
1122 }
1123 inline CylindricalCoords <T> & operator = (const SphericalCoords <T> & s)
1124 {
1125 PolarCoords <T>::operator = (s);
1126 z = s.r * cos(s.zenith);
1127 return *this;
1128 }
1129 template <typename J>
1130 inline operator const vector3D <J> () const
1131 {
1132 return vector3D <J> (
1133 static_cast <J> (static_cast <double> (this->p) * cos(this->azimuth)),
1134 static_cast <J> (static_cast <double> (this->p) * sin(this->azimuth)),
1135 static_cast <J> (z)
1136 );
1137 }
1138 inline operator const SphericalCoords <T> () const
1139 {
1140 return SphericalCoords <T>
1141 (
1142 sqrt(SQ(this->p) + SQ(z)),
1143 atan2(this->p, z),
1144 this->azimuth
1145 );
1146 }
1147 };
1148
1149 template <typename T>
1150 class Matrix3D
1151 {
1152 public:
1153 typedef T Precision;
1154
1155 T values [3][3];
1156
1157 inline Matrix3D (const T a1, const T b2, const T c3)
1158 {
1159 values[0][0] = a1; values[0][1] = 0; values[0][2] = 0;
1160 values[1][0] = 0; values[1][1] = b2; values[1][2] = 0;
1161 values[2][0] = 0; values[2][1] = 0; values[2][2] = c3;
1162 }
1163 inline Matrix3D (
1164 const T a1, const T b1, const T c1,
1165 const T a2, const T b2, const T c2,
1166 const T a3, const T b3, const T c3
1167 )
1168 {
1169 values[0][0] = a1; values[0][1] = b1; values[0][2] = c1;
1170 values[1][0] = a2; values[1][1] = b2; values[1][2] = c2;
1171 values[2][0] = a3; values[2][1] = b3; values[2][2] = c3;
1172 }
1173 inline Matrix3D (
1174 const vector3D<T> & a, vector3D<T> b, vector3D<T> c
1175 )
1176 {
1177 values[0][0] = a.x; values[0][1] = a.y; values[0][2] = a.z;
1178 values[1][0] = b.x; values[1][1] = b.y; values[1][2] = b.z;
1179 values[2][0] = c.x; values[2][1] = c.y; values[2][2] = c.z;
1180 }
1181
1182 inline operator T ()
1183 {
1184 return
1185 values[0][0] * values[1][1] * values[2][2] -
1186 values[0][0] * values[1][2] * values[2][1] +
1187 values[0][1] * values[1][2] * values[2][0] -
1188 values[0][1] * values[1][0] * values[2][2] +
1189 values[0][2] * values[1][0] * values[2][1] -
1190 values[0][2] * values[1][1] * values[2][0];
1191 }
1192 inline mars::vector3D <T> operator * (const mars::vector3D <T> v) const
1193 {
1194 return vector3D<T> (
1195 v.x * values[0][0] + v.y * values[0][1] + v.z * values[0][2],
1196 v.x * values[1][0] + v.y * values[1][1] + v.z * values[1][2],
1197 v.x * values[2][0] + v.y * values[2][1] + v.z * values[2][2]
1198 );
1199 }
1200 inline mars::Matrix3D <T> transpose () const
1201 {
1202 return Matrix3D (
1203 values[0][0], values[1][0], values[2][0],
1204 values[0][1], values[1][1], values[2][1],
1205 values[0][2], values[1][2], values[2][2]
1206 );
1207 }
1208 static inline mars::Matrix3D <T> rotateZ (float theta)
1209 {
1210 return Matrix3D <T> (
1211 cos(theta), -sin(theta), 0,
1212 sin(theta), cos(theta), 0,
1213 0, 0, 1
1214 );
1215 }
1216 };
1217
1218 template <typename T>
1219 class Quaternion : public DescQ< T, Quaternion< T > >
1220 {
1221 public:
1222 inline Quaternion (const T w, const T x, const T y, const T z)
1223 : DescQ< T, Quaternion< T > >(x, y, z, w) {}
1224 inline Quaternion (const vector3D <T> & vec)
1225 : DescQ< T, Quaternion< T > >(vec) {}
1226 inline Quaternion (const T s, const vector3D <T> & v)
1227 : DescQ< T, Quaternion< T > >(s, v) {}
1228 inline Quaternion ()
1229 : DescQ< T, Quaternion< T > >() {}
1230
1231 static
1232 inline Quaternion <T>
1233 rotation (const T theta, const vector3D <T> & u)
1234 {
1235 return Quaternion <T> (
1236 cos(theta / 2),
1237 u * sin(theta / 2)
1238 );
1239 }
1240
1241 inline const Quaternion <T> operator * (const Quaternion <T> & q) const
1242 {
1243 const T s1 = this->w, s2 = q.w;
1244 const vector3D <T> & v1 = this->v(), v2 = q.v();
1245
1246 return Quaternion <T> (
1247 s1 * s2 - (v1 * v2),
1248 (s1 * v2) + (s2 * v1) + (v1 & v2)
1249 );
1250 }
1251 inline const Quaternion <T> operator - () const
1252 {
1253 return Quaternion <T> (this->w, -this->x, -this->y, -this->z);
1254 }
1255 inline const Quaternion <T> operator ! () const
1256 {
1257 const Quaternion <T> qq = operator - ();
1258 return qq / (operator * (qq));
1259 }
1260 inline const Quaternion <T> operator / (const Quaternion <T> & q) const
1261 {
1262 const T divisor = (q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
1263 return Quaternion <T> (
1264 (q.x*this->x + q.y*this->y + q.z*this->z + q.w*this->w) / divisor,
1265 (this->x*q.w - this->y*q.z + this->z*q.y - this->w*q.x) / divisor,
1266 (this->x*q.z + this->y*q.w - this->z*q.x - this->w*q.y) / divisor,
1267 (this->y*q.x + this->z*q.w - this->x*q.y - this->w*q.z) / divisor
1268 );
1269 }
1270 inline const Quaternion <T> operator & (const Quaternion <T> & q) const
1271 {
1272 return operator * (q * operator ! ());
1273 }
1274 inline operator vector3D <T> () const
1275 {
1276 return this->v();
1277 }
1278 };
1279}
1280
1281#endif
Note: See TracBrowser for help on using the repository browser.