math3d.h
Go to the documentation of this file.
1 /********************************************************************************
2 Copyright (c) 2015, TRACLabs, Inc.
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6  are permitted provided that the following conditions are met:
7 
8  1. Redistributions of source code must retain the above copyright notice,
9  this list of conditions and the following disclaimer.
10 
11  2. Redistributions in binary form must reproduce the above copyright notice,
12  this list of conditions and the following disclaimer in the documentation
13  and/or other materials provided with the distribution.
14 
15  3. Neither the name of the copyright holder nor the names of its contributors
16  may be used to endorse or promote products derived from this software
17  without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
23 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24 BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
26 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
27 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
28 OF THE POSSIBILITY OF SUCH DAMAGE.
29 ********************************************************************************/
30 
31 #ifndef MATH3D_H
32 #define MATH3D_H
33 
34 #include <string>
35 #include <iostream>
36 #include <cmath>
37 #include <vector>
38 #include <stdexcept>
39 #include <cstdlib>
40 
41 #ifndef M_PI
42 #define M_PI (3.14159265358979323846)
43 #endif
44 
45 typedef unsigned char uint8_t;
46 typedef unsigned int uint32_t;
47 
48 namespace math3d
49 {
50 
51 static const double pi = M_PI;
52 static const double rad_on_deg = pi / 180.;
53 static const double deg_on_rad = 180. / pi;
54 
55 template <typename T>
56 inline bool almost_zero(T a, double e)
57 {
58  return (a == T(0)) || (a > 0 && a < e) || (a < 0 && a > -e);
59 }
60 
62 {
63  uint8_t r, g, b;
64  color_rgb24(uint8_t R, uint8_t G, uint8_t B) : r(R), g(G), b(B) {}
65 };
66 
67 template <typename T>
68 struct vec3d
69 {
70  T x, y, z;
71 
72  explicit vec3d() : x(0), y(0), z(0) {}
73  vec3d(T x_, T y_, T z_) : x(x_), y(y_), z(z_) {}
74 
75  template <typename S>
76  vec3d(const vec3d<S>& s) : x(T(s.x)), y(T(s.y)), z(T(s.z)) {}
77 
78  template <typename S>
79  vec3d(const S* s) : x(T(s[0])), y(T(s[1])), z(T(s[2])) {}
80 
81  vec3d<T> operator+(const vec3d<T>& p) const
82  {
83  return vec3d<T>(x + p.x, y + p.y, z + p.z);
84  }
85 
86  vec3d<T> operator-(const vec3d<T>& p) const
87  {
88  return vec3d<T>(x - p.x, y - p.y, z - p.z);
89  }
90 
92  {
93  return vec3d<T>(-x, -y, -z);
94  }
95 
96  template <typename S>
98  {
99  x += T(p.x);
100  y += T(p.y);
101  z += T(p.z);
102  return *this;
103  }
104 
105  // rules for partial ordering of function templates say that the new overload
106  // is a better match, when it matches
107 
109  {
110  x += p.x;
111  y += p.y;
112  z += p.z;
113  return *this;
114  }
115 
116  template <typename S>
118  {
119  x -= T(p.x);
120  y -= T(p.y);
121  z -= T(p.z);
122  return *this;
123  }
124 
126  {
127  x -= p.x;
128  y -= p.y;
129  z -= p.z;
130  return *this;
131  }
132 
133  template <typename Scalar>
134  vec3d<T>& operator/=(const Scalar& s)
135  {
136  const T i = T(1) / T(s);
137  x *= i;
138  y *= i;
139  z *= i;
140  return *this;
141  }
142 
143  template <typename Scalar>
144  vec3d<T>& operator*=(const Scalar& s)
145  {
146  x *= T(s);
147  y *= T(s);
148  z *= T(s);
149  return *this;
150  }
151 
152  bool operator==(const vec3d& o) const
153  {
154  return (x == o.x) && (y == o.y) && (z == o.z);
155  }
156 
157  template <typename S>
158  bool operator==(const vec3d<S>& o) const
159  {
160  return (x == T(o.x) && y == T(o.y) && z == T(o.z));
161  }
162 
163  bool operator!=(const vec3d& o) const
164  {
165  return !(*this == o);
166  }
167 
168  template <typename S>
169  bool operator!=(const vec3d<S>& o) const
170  {
171  return !(*this == o);
172  }
173 
174  template <typename Scalar>
175  friend vec3d<T> operator*(const vec3d<T>& p, const Scalar& s)
176  {
177  return vec3d<T>(s * p.x, s * p.y, s * p.z);
178  }
179 
180  template <typename Scalar>
181  friend vec3d<T> operator*(const Scalar& s, const vec3d<T>& p)
182  {
183  return p * s;
184  }
185 
186  template <typename Scalar>
187  friend vec3d<T> operator/(const vec3d<T>& p, const Scalar& s)
188  {
189  return vec3d<T>(p.x / T(s), p.y / T(s), p.z / T(s));
190  }
191 
192  friend std::ostream& operator<<(std::ostream& os, const vec3d<T>& p)
193  {
194  return (os << p.x << " " << p.y << " " << p.z);
195  }
196 
197  friend std::istream& operator>>(std::istream& is, vec3d<T>& p)
198  {
199  return (is >> p.x >> p.y >> p.z);
200  }
201 
202 };
203 
206 
207 class oriented_point3d : public point3d
208 {
209 public:
210  normal3d n;
211 
212  explicit oriented_point3d() : point3d() {}
213  oriented_point3d(const point3d& p) : point3d(p) {}
214  oriented_point3d(double xx, double yy, double zz) : point3d(xx, yy, zz) {}
215  oriented_point3d(const oriented_point3d& p) : point3d(p), n(p.n) {}
216  oriented_point3d(const point3d& p, const normal3d& nn) : point3d(p), n(nn) {}
217 };
218 
219 
220 struct triangle
221 {
223  int id0, id1, id2; // indices to vertices p0, p1, p2
224  normal3d n;
225  triangle() : id0(-1), id1(-1), id2(-1) {}
226  triangle(int id0, int id1, int id2) : id0(id0), id1(id1), id2(id2) {}
227  triangle(const oriented_point3d& p0_, const oriented_point3d& p1_, const oriented_point3d& p2_, const normal3d& n_)
228  : p0(p0_), p1(p1_), p2(p2_), n(n_) {}
229  triangle(const oriented_point3d& p0_, const oriented_point3d& p1_, const oriented_point3d& p2_, const normal3d& n_, int id0_, int id1_, int id2_)
230  : p0(p0_), p1(p1_), p2(p2_), id0(id0_), id1(id1_), id2(id2_), n(n_) {}
231  triangle(const point3d& p0_, const point3d& p1_, const point3d& p2_, const normal3d& n_)
232  : p0(p0_), p1(p1_), p2(p2_), n(n_) {}
233  friend std::ostream& operator<<(std::ostream& o, const triangle& t)
234  {
235  o << t.p0 << " " << t.p1 << " " << t.p2;
236  return o;
237  }
238 };
239 
240 
241 class invalid_vector : public std::logic_error
242 {
243 public:
244  explicit invalid_vector() : std::logic_error("Exception invalid_vector caught.") {}
245  invalid_vector(const std::string& msg) : std::logic_error("Exception invalid_vector caught: " + msg) {}
246 };
247 
248 
249 // ==============================================================
250 // Rotations
251 // ==============================================================
252 
253 // NOTE: this is a std::vector derivation, thus a matrix<bool> will
254 // take 1 bit per element.
255 
256 template<class T>
257 class matrix : private std::vector<T> // row-major order
258 {
259 private:
260  typedef std::vector<T> super;
261  int width_;
262  int height_;
263 
264 public:
265 
266  const int& width;
267  const int& height;
268 
269  typedef typename super::iterator iterator;
270  typedef typename super::const_iterator const_iterator;
271 
272  explicit matrix() : super(), width_(0), height_(0), width(width_), height(height_) {}
273 
274  matrix(int w, int h) : super(w * h), width_(w), height_(h), width(width_), height(height_) {}
275 
276  matrix(int w, int h, const T& v) : super(w * h, v), width_(w), height_(h), width(width_), height(height_) {}
277 
278  matrix(const matrix<T>& m) : super(), width(width_), height(height_)
279  {
280  resize(m.width_, m.height_);
281  super::assign(m.begin(), m.end());
282  }
283 
284  typename super::reference operator()(size_t r, size_t c)
285  {
286  return super::operator[](r * width_ + c);
287  }
288  typename super::const_reference operator()(size_t r, size_t c) const
289  {
290  return super::operator[](r * width_ + c);
291  }
292 
293  typename super::reference at(const size_t r, const size_t c)
294  {
295  return super::at(r * width_ + c);
296  }
297  typename super::const_reference at(size_t r, size_t c) const
298  {
299  return super::at(r * width_ + c);
300  }
301 
302  const T* to_ptr() const
303  {
304  return &(super::operator[](0)); // ok since std::vector is guaranteed to be contiguous
305  }
306 
307  T* to_ptr()
308  {
309  return &(super::operator[](0));
310  }
311 
312  void resize(int w, int h)
313  {
314  super::resize(w * h);
315  width_ = w;
316  height_ = h;
317  }
318 
319  size_t size() const
320  {
321  return super::size();
322  }
323 
324  iterator begin()
325  {
326  return super::begin();
327  }
328  const_iterator begin() const
329  {
330  return super::begin();
331  }
332  iterator end()
333  {
334  return super::end();
335  }
336  const_iterator end() const
337  {
338  return super::end();
339  }
340 
341  bool operator==(const matrix<T>& m) const
342  {
343  if ((width_ != m.width_) || (height != m.height_))
344  return false;
345 
346  const_iterator it1(begin()), it2(m.begin()), it1_end(end());
347 
348  for (; it1 != it1_end; ++it1, ++it2)
349  if (*it1 != *it2) return false;
350 
351  return true;
352  }
353 
354  bool operator!=(const matrix<T>& m) const
355  {
356  return !(*this == m);
357  }
358 
360  {
361  if (&m == this)
362  return *this;
363 
364  if (width != m.width || height != m.height)
365  throw invalid_vector("Cannot assign matrices with different sizes.");
366 
367  super::assign(m.begin(), m.end());
368  return *this;
369  }
370 
371  template <typename S>
372  matrix<T>& operator*=(const S& s)
373  {
374  for (size_t i = 0; i < size(); ++i)
375  super::operator[](i) *= T(s);
376  return *this;
377  }
378 
379  template <typename S>
380  matrix<T>& operator/=(const S& s)
381  {
382  for (size_t i = 0; i < size(); ++i)
383  super::operator[](i) /= T(s);
384  return *this;
385  }
386 
387  friend std::ostream& operator<<(std::ostream& s, const matrix<T>& m)
388  {
389  for (int y = 0; y < m.height_; ++y)
390  {
391  for (int x = 0; x < m.width_; ++x)
392  {
393  s << m[y * m.width_ + x] << " ";
394  }
395  s << std::endl;
396  }
397  return s;
398  }
399 };
400 
401 
402 template<typename T>
403 struct matrix3x3
404 {
405  T r00, r01, r02,
406  r10, r11, r12,
407  r20, r21, r22;
408 
409  int width, height;
410 
411  explicit matrix3x3()
412  : r00(0), r01(0), r02(0)
413  , r10(0), r11(0), r12(0)
414  , r20(0), r21(0), r22(0)
415  , width(3), height(3)
416  {}
417 
418  template <typename S>
419  explicit matrix3x3(const S* v)
420  : r00(v[0]), r01(v[1]), r02(v[2])
421  , r10(v[3]), r11(v[4]), r12(v[5])
422  , r20(v[6]), r21(v[7]), r22(v[8])
423  , width(3), height(3)
424  {}
425 
426  void set_column(size_t c, const vec3d<T>& v)
427  {
428  T x = v.x;
429  T y = v.y;
430  T z = v.z;
431 
432  if (c == 0)
433  {
434  r00 = x;
435  r10 = y;
436  r20 = z;
437  }
438  else if (c == 1)
439  {
440  r01 = x;
441  r11 = y;
442  r21 = z;
443  }
444  else if (c == 2)
445  {
446  r02 = x;
447  r12 = y;
448  r22 = z;
449  }
450  else
451  throw std::logic_error("Cannot set column for 3x3 matrix.");
452  }
453 
454  T& operator()(size_t row, size_t col)
455  {
456  switch (row)
457  {
458  case 0:
459  if (col == 0) return r00;
460  if (col == 1) return r01;
461  if (col == 2) return r02;
462  else throw std::out_of_range("Cannot access element in 3x3 matrix");
463  case 1:
464  if (col == 0) return r10;
465  if (col == 1) return r11;
466  if (col == 2) return r12;
467  else throw std::out_of_range("Cannot access element in 3x3 matrix");
468  case 2:
469  if (col == 0) return r20;
470  if (col == 1) return r21;
471  if (col == 2) return r22;
472  else throw std::out_of_range("Cannot access element in 3x3 matrix");
473  default:
474  throw std::out_of_range("Cannot access element in 3x3 matrix");
475  }
476  }
477 
478  const T& operator()(size_t row, size_t col) const
479  {
480  switch (row)
481  {
482  case 0:
483  if (col == 0) return r00;
484  if (col == 1) return r01;
485  if (col == 2) return r02;
486  else throw std::out_of_range("Cannot access element in 3x3 matrix");
487  case 1:
488  if (col == 0) return r10;
489  if (col == 1) return r11;
490  if (col == 2) return r12;
491  else throw std::out_of_range("Cannot access element in 3x3 matrix");
492  case 2:
493  if (col == 0) return r20;
494  if (col == 1) return r21;
495  if (col == 2) return r22;
496  else throw std::out_of_range("Cannot access element in 3x3 matrix");
497  default:
498  throw std::out_of_range("Cannot access element in 3x3 matrix");
499  }
500  }
501 
502  friend std::ostream& operator<<(std::ostream& s, const matrix3x3<T>& m)
503  {
504  s << m.r00 << " " << m.r01 << " " << m.r02 << std::endl;
505  s << m.r10 << " " << m.r11 << " " << m.r12 << std::endl;
506  s << m.r20 << " " << m.r21 << " " << m.r22 << std::endl;
507  return s;
508  }
509 };
510 
511 
512 template <typename T>
514 {
515  matrix3x3<T> m;
516  set_identity(m);
517  return m;
518 }
519 
520 
521 template <typename T>
522 inline void mult_matrix_inplace(const matrix3x3<T>& m1, const matrix3x3<T>& m2, matrix3x3<T>& r)
523 {
524  const double r00 = m1.r00 * m2.r00 + m1.r01 * m2.r10 + m1.r02 * m2.r20;
525  const double r01 = m1.r00 * m2.r01 + m1.r01 * m2.r11 + m1.r02 * m2.r21;
526  const double r02 = m1.r00 * m2.r02 + m1.r01 * m2.r12 + m1.r02 * m2.r22;
527 
528  const double r10 = m1.r10 * m2.r00 + m1.r11 * m2.r10 + m1.r12 * m2.r20;
529  const double r11 = m1.r10 * m2.r01 + m1.r11 * m2.r11 + m1.r12 * m2.r21;
530  const double r12 = m1.r10 * m2.r02 + m1.r11 * m2.r12 + m1.r12 * m2.r22;
531 
532  const double r20 = m1.r20 * m2.r00 + m1.r21 * m2.r10 + m1.r22 * m2.r20;
533  const double r21 = m1.r20 * m2.r01 + m1.r21 * m2.r11 + m1.r22 * m2.r21;
534  const double r22 = m1.r20 * m2.r02 + m1.r21 * m2.r12 + m1.r22 * m2.r22;
535 
536  r.r00 = r00;
537  r.r01 = r01;
538  r.r02 = r02;
539  r.r10 = r10;
540  r.r11 = r11;
541  r.r12 = r12;
542  r.r20 = r20;
543  r.r21 = r21;
544  r.r22 = r22;
545 }
546 
547 template <typename T>
548 inline void mult_matrix(const matrix3x3<T>& m1, const matrix3x3<T>& m2, matrix3x3<T>& r)
549 {
550  if (&r == &m1 || &r == &m2) throw std::logic_error("math3d::mult_matrix() argument alias");
551  return mult_matrix_inplace<T>(m1, m2, r);
552 }
553 
554 // NO in-place!
555 template <typename Rot1, typename Rot2, typename Rot3>
556 void mult_matrix(const Rot1& m1, const Rot2& m2, Rot3& r)
557 {
558  if ((char*)&r == (char*)&m1 || (char*)&r == (char*)&m2)
559  throw std::logic_error("math3d::mult_matrix() argument alias");
560 
561  if (m1.width != m2.height || r.height != m1.height || r.width != m2.width)
562  throw std::logic_error("Incompatible size matrices");
563 
564  double sum;
565 
566  for (int is = 0; is < m1.height; ++is)
567  {
568  for (int jd = 0; jd < m2.width; ++jd)
569  {
570  sum = 0.;
571  for (int js = 0; js < m1.width; ++js)
572  {
573  sum += m1(is, js) * m2(js, jd);
574  }
575  r(is, jd) = sum;
576  }
577  }
578 }
579 
580 // ==============================================================
581 // Quaternions
582 // ==============================================================
583 
584 template <typename T>
586 {
587  T w, i, j, k;
588 
589  explicit quaternion(T v = 0) : w(v), i(0), j(0), k(0) {}
590  //explicit quaternion(const T* p) : w(p[0]), i(p[1]), j(p[2]), k(p[3]) {}
591  quaternion(T ww, T ii, T jj, T kk) : w(ww), i(ii), j(jj), k(kk) {}
592 
593  static quaternion<T> convert(const vec3d<T>& p)
594  {
595  return quaternion<T>(0, p.x, p.y, p.z);
596  }
597 
598  static quaternion<T> convert(const T* p)
599  {
600  return quaternion<T>(p[0], p[1], p[2], p[3]);
601  }
602 
603  quaternion<T>& operator+= (const quaternion<T>& a)
604  {
605  w += a.w;
606  i += a.i;
607  j += a.j;
608  k += a.k;
609  return *this;
610  }
611 
612  quaternion<T>& operator*= (T a)
613  {
614  w *= a;
615  i *= a;
616  j *= a;
617  k *= a;
618  return *this;
619  }
620 
621  void to_vector(T* p) const
622  {
623  p[0] = w;
624  p[1] = i;
625  p[2] = j;
626  p[3] = k;
627  }
628 
629  friend std::ostream& operator<<(std::ostream& os, const quaternion<T>& q)
630  {
631  return os << "[ " << q.w << " " << q.i << " " << q.j << " " << q.k << " ]";
632  }
633 
634  friend std::istream& operator>>(std::istream& is, quaternion<T>& q)
635  {
636  std::string dump;
637  return (is >> dump >> q.w >> q.i >> q.j >> q.k >> dump);
638  }
639 };
640 
641 template <typename T>
643 {
644  quaternion<T> result(a);
645  result += b;
646  return result;
647 }
648 
649 template <typename T>
650 T dot(const quaternion<T>& a, const quaternion<T>& b)
651 {
652  return a.w * b.w + a.i * b.i + a.j * b.j + a.k * b.k;
653 }
654 
655 template <typename T>
656 T norm(const quaternion<T>& a)
657 {
658  return std::sqrt(dot(a, a));
659 }
660 
661 template <typename T>
663 {
664  quaternion<T> result;
665 
666  result.w = a.w * b.w - a.i * b.i - a.j * b.j - a.k * b.k;
667  result.i = a.i * b.w + a.w * b.i - a.k * b.j + a.j * b.k;
668  result.j = a.j * b.w + a.k * b.i + a.w * b.j - a.i * b.k;
669  result.k = a.k * b.w - a.j * b.i + a.i * b.j + a.w * b.k;
670 
671  return result;
672 }
673 
674 template <typename T>
676 {
677  return quaternion<T>(a.w, -a.i, -a.j, -a.k);
678 }
679 
680 template <typename T>
681 inline void conjugate(quaternion<T>& q)
682 {
683  q.i = -q.i;
684  q.j = -q.j;
685  q.k = -q.k;
686 }
687 
688 template <typename T>
689 inline void normalize(quaternion<T>& q)
690 {
691  T mag = q.w * q.w + q.i * q.i + q.j * q.j + q.k * q.k;
692  if (!almost_zero(mag - T(1), 1e-10))
693  {
694  mag = std::sqrt(mag);
695  q.w /= mag;
696  q.i /= mag;
697  q.j /= mag;
698  q.k /= mag;
699  }
700 }
701 
702 template <typename T>
704 {
705  q.w = T(1);
706  q.i = q.j = q.k = T(0);
707 }
708 
709 // returns a normalized unit quaternion
710 template<typename T>
712 {
713  const T m00 = m(0, 0);
714  const T m11 = m(1, 1);
715  const T m22 = m(2, 2);
716  const T m01 = m(0, 1);
717  const T m02 = m(0, 2);
718  const T m10 = m(1, 0);
719  const T m12 = m(1, 2);
720  const T m20 = m(2, 0);
721  const T m21 = m(2, 1);
722  const T tr = m00 + m11 + m22;
723 
724  quaternion<T> ret;
725 
726  if (tr > 0)
727  {
728  T s = std::sqrt(tr + T(1)) * 2; // S=4*qw
729  ret.w = 0.25 * s;
730  ret.i = (m21 - m12) / s;
731  ret.j = (m02 - m20) / s;
732  ret.k = (m10 - m01) / s;
733  }
734  else if ((m00 > m11) & (m00 > m22))
735  {
736  const T s = std::sqrt(T(1) + m00 - m11 - m22) * 2; // S=4*qx
737  ret.w = (m21 - m12) / s;
738  ret.i = 0.25 * s;
739  ret.j = (m01 + m10) / s;
740  ret.k = (m02 + m20) / s;
741  }
742  else if (m11 > m22)
743  {
744  const T s = std::sqrt(T(1) + m11 - m00 - m22) * 2; // S=4*qy
745  ret.w = (m02 - m20) / s;
746  ret.i = (m01 + m10) / s;
747  ret.j = 0.25 * s;
748  ret.k = (m12 + m21) / s;
749  }
750  else
751  {
752  const T s = std::sqrt(T(1) + m22 - m00 - m11) * 2; // S=4*qz
753  ret.w = (m10 - m01) / s;
754  ret.i = (m02 + m20) / s;
755  ret.j = (m12 + m21) / s;
756  ret.k = 0.25 * s;
757  }
758 
759  return ret;
760 }
761 
762 // assumes a normalized unit quaternion
763 template <typename T>
765 {
766  matrix3x3<T> m;
767  const T w = q.w, i = q.i, j = q.j, k = q.k;
768 
769  m(0, 0) = 1 - 2 * j * j - 2 * k * k;
770  m(0, 1) = 2 * i * j - 2 * k * w;
771  m(0, 2) = 2 * i * k + 2 * j * w;
772 
773  m(1, 0) = 2 * i * j + 2 * k * w;
774  m(1, 1) = 1 - 2 * i * i - 2 * k * k;
775  m(1, 2) = 2 * j * k - 2 * i * w;
776 
777  m(2, 0) = 2 * i * k - 2 * j * w;
778  m(2, 1) = 2 * j * k + 2 * i * w;
779  m(2, 2) = 1 - 2 * i * i - 2 * j * j;
780 
781  return m;
782 }
783 
784 template <typename T>
786 {
787  r.w = a.w * b.w - (a.i * b.i + a.j * b.j + a.k * b.k);
788  r.i = a.w * b.i + b.w * a.i + a.j * b.k - a.k * b.j;
789  r.j = a.w * b.j + b.w * a.j + a.k * b.i - a.i * b.k;
790  r.k = a.w * b.k + b.w * a.k + a.i * b.j - a.j * b.i;
791 }
792 
793 // ==============================================================
794 //
795 // ==============================================================
796 
797 template <typename T>
799 {
800  matrix<T> old(m);
801  const int w = m.width;
802  const int h = m.height;
803  m.resize(h, w);
804 
805  for (int row = 0; row < h; ++row)
806  {
807  for (int col = 0; col < w; ++col)
808  {
809  m(col, row) = old(row, col);
810  }
811  }
812 }
813 
814 template <typename T>
815 inline void transpose(matrix3x3<T>& m)
816 {
817  const T m01 = m.r01, m02 = m.r02, m12 = m.r12, m10 = m.r10, m21 = m.r21, m20 = m.r20;
818  m.r01 = m10;
819  m.r02 = m20;
820  m.r10 = m01;
821  m.r20 = m02;
822  m.r12 = m21;
823  m.r21 = m12;
824 }
825 
826 template <typename T>
828 {
829  matrix3x3<T> ret;
830  ret.r00 = m.r00;
831  ret.r01 = m.r10;
832  ret.r02 = m.r20;
833  ret.r10 = m.r01;
834  ret.r11 = m.r11;
835  ret.r20 = m.r02;
836  ret.r12 = m.r21;
837  ret.r21 = m.r12;
838  ret.r22 = m.r22;
839  return ret;
840 }
841 
842 // dest matrix must already be of the right size
843 template <typename T>
844 void transpose(const matrix<T>& src, matrix<T>& dest)
845 {
846  if (src.width != dest.height || src.height != dest.width)
847  throw math3d::invalid_vector("math3d::transpose(): Destination matrix must be of the right size.");
848 
849  const int w = src.width;
850  const int h = src.height;
851 
852  for (int row = 0; row < h; ++row)
853  {
854  for (int col = 0; col < w; ++col)
855  {
856  dest(col, row) = src(row, col);
857  }
858  }
859 }
860 
861 template <typename T>
862 inline void transpose(const matrix3x3<T>& src, matrix3x3<T>& dest)
863 {
864  dest.r00 = src.r00;
865  dest.r11 = src.r11;
866  dest.r22 = src.r22;
867  dest.r01 = src.r10;
868  dest.r02 = src.r20;
869  dest.r10 = src.r01;
870  dest.r12 = src.r21;
871  dest.r21 = src.r12;
872  dest.r20 = src.r02;
873 }
874 
875 template <typename T>
876 void set_identity(matrix<T>& m, T val = 1)
877 {
878  if (m.width != m.height)
879  throw invalid_vector("Cannot set identity on a rectangular matrix.");
880 
881  if (m.width == 0)
882  return;
883 
884  const int n = m.width * m.height;
885  const int w = m.width;
886 
887  int one = 0;
888  for (int k = 0; k < n; ++k)
889  {
890  if (k == one)
891  {
892  m(k / w, k % w) = val;
893  one += w + 1;
894  }
895  else
896  m(k / w, k % w) = 0;
897  }
898 }
899 
900 template <typename T>
901 void set_identity(matrix3x3<T>& m, T val = 1)
902 {
903  m.r00 = val;
904  m.r01 = 0;
905  m.r02 = 0;
906  m.r10 = 0;
907  m.r11 = val;
908  m.r12 = 0;
909  m.r20 = 0;
910  m.r21 = 0;
911  m.r22 = val;
912 }
913 
914 
915 // ==============================================================
916 // Rigid Motion
917 // ==============================================================
918 
919 typedef std::pair<matrix3x3<double>, point3d> rigid_motion_t;
920 
921 template <typename T>
922 void rotate(vec3d<T>& p, const matrix<T>& rot)
923 {
924  if (rot.height != 3 || rot.width != 3)
925  throw std::logic_error("Rotation matrix must be 3x3");
926 
927  T oldx = p.x, oldy = p.y, oldz = p.z;
928  p.x = oldx * rot(0, 0) + oldy * rot(0, 1) + oldz * rot(0, 2);
929  p.y = oldx * rot(1, 0) + oldy * rot(1, 1) + oldz * rot(1, 2);
930  p.z = oldx * rot(2, 0) + oldy * rot(2, 1) + oldz * rot(2, 2);
931 }
932 
933 template <typename T>
934 void rotate(vec3d<T>& p, const matrix3x3<T>& rot)
935 {
936  T oldx = p.x, oldy = p.y, oldz = p.z;
937  p.x = oldx * rot.r00 + oldy * rot.r01 + oldz * rot.r02;
938  p.y = oldx * rot.r10 + oldy * rot.r11 + oldz * rot.r12;
939  p.z = oldx * rot.r20 + oldy * rot.r21 + oldz * rot.r22;
940 }
941 
942 template <typename T, typename S>
943 void rotate(vec3d<T>& p, const matrix<S>& rot)
944 {
945  if (rot.height != 3 || rot.width != 3)
946  throw std::logic_error("Rotation matrix must be 3x3");
947 
948  T oldx = p.x, oldy = p.y, oldz = p.z;
949  p.x = T(oldx * rot(0, 0) + oldy * rot(0, 1) + oldz * rot(0, 2));
950  p.y = T(oldx * rot(1, 0) + oldy * rot(1, 1) + oldz * rot(1, 2));
951  p.z = T(oldx * rot(2, 0) + oldy * rot(2, 1) + oldz * rot(2, 2));
952 }
953 
954 template <typename T, typename S>
955 inline void rotate(vec3d<T>& p, const matrix3x3<S>& rot)
956 {
957  T oldx = p.x, oldy = p.y, oldz = p.z;
958  p.x = T(oldx * rot.r00 + oldy * rot.r01 + oldz * rot.r02);
959  p.y = T(oldx * rot.r10 + oldy * rot.r11 + oldz * rot.r12);
960  p.z = T(oldx * rot.r20 + oldy * rot.r21 + oldz * rot.r22);
961 }
962 
963 template <typename T>
964 inline void rotate(vec3d<T>& p, const quaternion<T>& rot)
965 {
967 }
968 
969 
970 template <typename T>
971 inline vec3d<T> get_rotate(const vec3d<T>& v, const quaternion<T>& q)
972 {
973  return get_rotate(v, quaternion_to_rot_matrix(q));
974  /*
975  const T
976  a = q.w, b = q.i, c = q.j, d = q.k,
977  t2 = a*b, t3 = a*c, t4 = a*d, t5 = -b*b, t6 = b*c,
978  t7 = b*d, t8 = -c*c, t9 = c*d, t10 = -d*d,
979  v1 = v.x, v2 = v.y, v3 = v.z;
980  return vec3d<T>(
981  2*( (t8 + t10)*v1 + (t6 - t4)*v2 + (t3 + t7)*v3 ) + v1,
982  2*( (t4 + t6)*v1 + (t5 + t10)*v2 + (t9 - t2)*v3 ) + v2,
983  2*( (t7 - t3)*v1 + (t2 + t9)*v2 + (t5 + t8)*v3 ) + v3);
984  */
985 }
986 
987 template <typename T>
988 inline vec3d<T> get_rotate(const vec3d<T>& p, const matrix3x3<T>& rot)
989 {
990  return vec3d<T>(p.x * rot.r00 + p.y * rot.r01 + p.z * rot.r02,
991  p.x * rot.r10 + p.y * rot.r11 + p.z * rot.r12,
992  p.x * rot.r20 + p.y * rot.r21 + p.z * rot.r22);
993 }
994 
995 
996 template <typename T, typename RotationType>
997 inline void rotate_translate(vec3d<T>& v, const RotationType& rot, const point3d& trans)
998 {
999  rotate(v, rot);
1000  v += trans;
1001 }
1002 
1003 template <typename T>
1004 inline vec3d<T> get_rotate_translate(const vec3d<T>& p, const matrix3x3<T>& rot, const point3d& t)
1005 {
1006  return vec3d<T>(p.x * rot.r00 + p.y * rot.r01 + p.z * rot.r02 + t.x,
1007  p.x * rot.r10 + p.y * rot.r11 + p.z * rot.r12 + t.y,
1008  p.x * rot.r20 + p.y * rot.r21 + p.z * rot.r22 + t.z);
1009 }
1010 
1011 template <typename T>
1012 inline vec3d<T> get_rotate_translate(const vec3d<T>& p, const matrix<T>& rot, const point3d& t)
1013 {
1014  if (rot.height != 3 || rot.width != 3)
1015  throw std::logic_error("Rotation matrix must be 3x3");
1016 
1017  return vec3d<T>(p.x * rot(0, 0) + p.y * rot(0, 1) + p.z * rot(0, 2) + t.x,
1018  p.x * rot(1, 0) + p.y * rot(1, 1) + p.z * rot(1, 2) + t.y,
1019  p.x * rot(2, 0) + p.y * rot(2, 1) + p.z * rot(2, 2) + t.z);
1020 }
1021 
1022 template <typename T>
1023 inline vec3d<T> get_rotate_translate(const vec3d<T>& p, const T* rot, const T* t)
1024 {
1025  return get_rotate_translate(p, matrix3x3<T>(rot), vec3d<T>(t[0], t[1], t[2]));
1026 }
1027 
1028 template <typename T>
1029 inline vec3d<T> get_rotate_translate(const vec3d<T>& v, const quaternion<T>& rot, const point3d& t)
1030 {
1031  return (get_rotate(v, rot) + t);
1032 }
1033 
1034 
1038 template <typename R, typename T>
1039 inline void invert(R& r, T& t)
1040 {
1041  transpose(r);
1042  rotate(t, r);
1043  t.x = -t.x;
1044  t.y = -t.y;
1045  t.z = -t.z;
1046 }
1047 
1048 
1053 template <typename T>
1055 
1056  const matrix3x3<T>& Ri, const point3d& Ti,
1057  const matrix3x3<T>& Rj, const point3d& Tj,
1058  matrix3x3<T>& Rij, point3d& Tij
1059 
1060 )
1061 {
1062  matrix3x3<T> Ri_inv = Ri;
1063  point3d Ti_inv = Ti;
1064  invert(Ri_inv, Ti_inv);
1065 
1066  mult_matrix(Ri_inv, Rj, Rij);
1067  Tij = get_rotate_translate(Tj, Ri_inv, Ti_inv);
1068 }
1069 
1070 
1071 // ==============================================================
1072 // Vector Operations
1073 // ==============================================================
1074 
1075 template <typename T>
1076 inline double normalize(vec3d<T>& p)
1077 {
1078  const double n = magnitude(p);
1079  if (n == 0.)
1080  {
1081  p.x = 0;
1082  p.y = 0;
1083  p.z = 0;
1084  }
1085  else
1086  {
1087  p.x /= n;
1088  p.y /= n;
1089  p.z /= n;
1090  }
1091  return n;
1092 }
1093 
1094 template <typename T>
1096 {
1097  vec3d<T> q(p);
1098  normalize(q);
1099  return q;
1100 }
1101 
1102 template <typename T>
1103 inline double dist(const T& p1, const T& p2)
1104 {
1105  const double sqdist = squared_dist(p1, p2);
1106  return (sqdist == 0. ? 0. : std::sqrt(sqdist));
1107 }
1108 
1109 // ||p1 - p2||^2
1110 template <typename T>
1111 inline double squared_dist(const vec3d<T>& p1, const vec3d<T>& p2)
1112 {
1113  T x = p1.x - p2.x;
1114  T y = p1.y - p2.y;
1115  T z = p1.z - p2.z;
1116  return ((x * x) + (y * y) + (z * z));
1117 }
1118 
1119 template <typename T>
1120 inline T dot_product(const vec3d<T>& v1, const vec3d<T>& v2)
1121 {
1122  return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
1123 }
1124 
1125 template <typename T, typename S>
1126 inline double dot_product(const vec3d<T>& v1, const vec3d<S>& v2)
1127 {
1128  return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
1129 }
1130 
1131 template <typename T>
1132 inline T dot_product(const quaternion<T>& p, const quaternion<T>& q)
1133 {
1134  return (p.w * q.w + p.i * q.i + p.j * q.j + p.k * q.k);
1135 }
1136 
1137 template <typename T>
1138 inline double norm2(const T& v)
1139 {
1140  return dot_product(v, v);
1141 }
1142 
1143 template <typename T>
1144 inline double magnitude(const T& p)
1145 {
1146  return std::sqrt(dot_product(p, p));
1147 }
1148 
1149 template <typename T>
1150 inline vec3d<T> cross_product(const vec3d<T>& v1, const vec3d<T>& v2)
1151 {
1152  return vec3d<T>(
1153  (v1.y * v2.z) - (v1.z * v2.y),
1154  (v1.z * v2.x) - (v1.x * v2.z),
1155  (v1.x * v2.y) - (v1.y * v2.x)
1156  );
1157 }
1158 
1159 
1160 template <typename Iterator>
1161 inline double median(Iterator start, Iterator end)
1162 {
1163  const typename Iterator::difference_type n = end - start;
1164  if (n <= 0) return 0.;
1165 
1166  if (n % 2 == 0)
1167  return (*(start + (n / 2)) + * (start + (n / 2 - 1))) / 2.;
1168  else
1169  return *(start + ((n - 1) / 2));
1170 }
1171 
1172 }
1173 
1174 #endif
T & operator()(size_t row, size_t col)
Definition: math3d.h:454
vec3d< T > & operator-=(const vec3d< S > &p)
Definition: math3d.h:117
triangle(const point3d &p0_, const point3d &p1_, const point3d &p2_, const normal3d &n_)
Definition: math3d.h:231
const_iterator begin() const
Definition: math3d.h:328
matrix(int w, int h, const T &v)
Definition: math3d.h:276
Definition: math3d.h:48
static const double pi
Definition: math3d.h:51
void rotate_translate(vec3d< T > &v, const RotationType &rot, const point3d &trans)
Definition: math3d.h:997
bool operator==(const vec3d &o) const
Definition: math3d.h:152
void transpose(matrix< T > &m)
Definition: math3d.h:798
void resize(int w, int h)
Definition: math3d.h:312
iterator begin()
Definition: math3d.h:324
color_rgb24(uint8_t R, uint8_t G, uint8_t B)
Definition: math3d.h:64
bool operator!=(const vec3d &o) const
Definition: math3d.h:163
oriented_point3d p2
Definition: math3d.h:222
vec3d< double > point3d
Definition: math3d.h:205
oriented_point3d(const point3d &p, const normal3d &nn)
Definition: math3d.h:216
void invert(R &r, T &t)
Definition: math3d.h:1039
unsigned char uint8_t
Definition: math3d.h:45
triangle(int id0, int id1, int id2)
Definition: math3d.h:226
quaternion< T > operator~(const quaternion< T > &a)
Definition: math3d.h:675
vec3d< T > operator-(const vec3d< T > &p) const
Definition: math3d.h:86
XmlRpcServer s
quaternion(T ww, T ii, T jj, T kk)
Definition: math3d.h:591
friend std::istream & operator>>(std::istream &is, vec3d< T > &p)
Definition: math3d.h:197
std::pair< matrix3x3< double >, point3d > rigid_motion_t
Definition: math3d.h:919
#define M_PI
Definition: math3d.h:42
oriented_point3d(const oriented_point3d &p)
Definition: math3d.h:215
const int & width
Definition: math3d.h:266
vec3d(const S *s)
Definition: math3d.h:79
matrix & operator=(const matrix< T > &m)
Definition: math3d.h:359
oriented_point3d p1
Definition: math3d.h:222
invalid_vector(const std::string &msg)
Definition: math3d.h:245
double median(Iterator start, Iterator end)
Definition: math3d.h:1161
matrix3x3< T > identity3x3()
Definition: math3d.h:513
triangle(const oriented_point3d &p0_, const oriented_point3d &p1_, const oriented_point3d &p2_, const normal3d &n_)
Definition: math3d.h:227
static const double rad_on_deg
Definition: math3d.h:52
void to_vector(T *p) const
Definition: math3d.h:621
static const double deg_on_rad
Definition: math3d.h:53
matrix(const matrix< T > &m)
Definition: math3d.h:278
T dot_product(const vec3d< T > &v1, const vec3d< T > &v2)
Definition: math3d.h:1120
void set_column(size_t c, const vec3d< T > &v)
Definition: math3d.h:426
void set_identity(quaternion< T > &q)
Definition: math3d.h:703
super::reference operator()(size_t r, size_t c)
Definition: math3d.h:284
vec3d< T > & operator/=(const Scalar &s)
Definition: math3d.h:134
bool operator!=(const vec3d< S > &o) const
Definition: math3d.h:169
friend vec3d< T > operator/(const vec3d< T > &p, const Scalar &s)
Definition: math3d.h:187
const T * to_ptr() const
Definition: math3d.h:302
double dist(const T &p1, const T &p2)
Definition: math3d.h:1103
vec3d< T > operator+(const vec3d< T > &p) const
Definition: math3d.h:81
triangle(const oriented_point3d &p0_, const oriented_point3d &p1_, const oriented_point3d &p2_, const normal3d &n_, int id0_, int id1_, int id2_)
Definition: math3d.h:229
matrix< T > & operator/=(const S &s)
Definition: math3d.h:380
matrix3x3< T > get_transpose(const matrix3x3< T > &m)
Definition: math3d.h:827
friend std::istream & operator>>(std::istream &is, quaternion< T > &q)
Definition: math3d.h:634
T norm(const quaternion< T > &a)
Definition: math3d.h:656
vec3d(T x_, T y_, T z_)
Definition: math3d.h:73
normal3d n
Definition: math3d.h:224
const_iterator end() const
Definition: math3d.h:336
super::const_reference at(size_t r, size_t c) const
Definition: math3d.h:297
void relative_motion(const matrix3x3< T > &Ri, const point3d &Ti, const matrix3x3< T > &Rj, const point3d &Tj, matrix3x3< T > &Rij, point3d &Tij)
Definition: math3d.h:1054
super::const_iterator const_iterator
Definition: math3d.h:270
void rotate(vec3d< T > &p, const matrix< T > &rot)
Definition: math3d.h:922
T dot(const quaternion< T > &a, const quaternion< T > &b)
Definition: math3d.h:650
vec3d< T > & operator+=(const vec3d< S > &p)
Definition: math3d.h:97
bool operator==(const matrix< T > &m) const
Definition: math3d.h:341
static quaternion< T > convert(const T *p)
Definition: math3d.h:598
bool almost_zero(T a, double e)
Definition: math3d.h:56
vec3d< T > & operator+=(const vec3d< T > &p)
Definition: math3d.h:108
vec3d< T > get_normalize(const vec3d< T > &p)
Definition: math3d.h:1095
quaternion< T > operator*(const quaternion< T > &a, const quaternion< T > &b)
Definition: math3d.h:662
bool operator==(const vec3d< S > &o) const
Definition: math3d.h:158
iterator end()
Definition: math3d.h:332
oriented_point3d(const point3d &p)
Definition: math3d.h:213
T * to_ptr()
Definition: math3d.h:307
size_t size() const
Definition: math3d.h:319
quaternion< T > operator+(const quaternion< T > &a, const quaternion< T > &b)
Definition: math3d.h:642
vec3d< T > operator-() const
Definition: math3d.h:91
vec3d< T > & operator*=(const Scalar &s)
Definition: math3d.h:144
static quaternion< T > convert(const vec3d< T > &p)
Definition: math3d.h:593
unsigned int uint32_t
Definition: math3d.h:46
super::const_reference operator()(size_t r, size_t c) const
Definition: math3d.h:288
vec3d< T > cross_product(const vec3d< T > &v1, const vec3d< T > &v2)
Definition: math3d.h:1150
double norm2(const T &v)
Definition: math3d.h:1138
vec3d< T > get_rotate_translate(const vec3d< T > &p, const matrix3x3< T > &rot, const point3d &t)
Definition: math3d.h:1004
super::reference at(const size_t r, const size_t c)
Definition: math3d.h:293
double squared_dist(const vec3d< T > &p1, const vec3d< T > &p2)
Definition: math3d.h:1111
double magnitude(const T &p)
Definition: math3d.h:1144
void normalize(quaternion< T > &q)
Definition: math3d.h:689
oriented_point3d p0
Definition: math3d.h:222
void mult_matrix(const matrix3x3< T > &m1, const matrix3x3< T > &m2, matrix3x3< T > &r)
Definition: math3d.h:548
friend vec3d< T > operator*(const vec3d< T > &p, const Scalar &s)
Definition: math3d.h:175
const T & operator()(size_t row, size_t col) const
Definition: math3d.h:478
friend vec3d< T > operator*(const Scalar &s, const vec3d< T > &p)
Definition: math3d.h:181
vec3d< T > get_rotate(const vec3d< T > &v, const quaternion< T > &q)
Definition: math3d.h:971
const int & height
Definition: math3d.h:267
vec3d< double > normal3d
Definition: math3d.h:204
void mult_matrix_inplace(const matrix3x3< T > &m1, const matrix3x3< T > &m2, matrix3x3< T > &r)
Definition: math3d.h:522
quaternion< T > rot_matrix_to_quaternion(const matrix3x3< T > &m)
Definition: math3d.h:711
std::vector< T > super
Definition: math3d.h:260
vec3d(const vec3d< S > &s)
Definition: math3d.h:76
void conjugate(quaternion< T > &q)
Definition: math3d.h:681
bool operator!=(const matrix< T > &m) const
Definition: math3d.h:354
super::iterator iterator
Definition: math3d.h:269
friend std::ostream & operator<<(std::ostream &o, const triangle &t)
Definition: math3d.h:233
matrix3x3(const S *v)
Definition: math3d.h:419
void mult_quaternion(const quaternion< T > &a, const quaternion< T > &b, quaternion< T > &r)
Definition: math3d.h:785
oriented_point3d(double xx, double yy, double zz)
Definition: math3d.h:214
matrix(int w, int h)
Definition: math3d.h:274
matrix3x3< T > quaternion_to_rot_matrix(const quaternion< T > &q)
Definition: math3d.h:764
quaternion(T v=0)
Definition: math3d.h:589
vec3d< T > & operator-=(const vec3d< T > &p)
Definition: math3d.h:125
matrix< T > & operator*=(const S &s)
Definition: math3d.h:372


trac_ik_lib
Author(s): Patrick Beeson, Barrett Ames
autogenerated on Sat Sep 19 2020 03:40:57