Thea
MatrixUtil.hpp
1 //============================================================================
2 //
3 // This file is part of the Thea toolkit.
4 //
5 // This software is distributed under the BSD license, as detailed in the
6 // accompanying LICENSE.txt file. Portions are derived from other works:
7 // their respective licenses and copyright information are reproduced in
8 // LICENSE.txt and/or in the relevant source files.
9 //
10 // Author: Siddhartha Chaudhuri
11 // First version: 2011
12 //
13 //============================================================================
14 
15 #ifndef __Thea_MatrixUtil_hpp__
16 #define __Thea_MatrixUtil_hpp__
17 
18 #include "Common.hpp"
19 #include "Math.hpp"
20 #include "MatVec.hpp"
21 #include <type_traits>
22 
23 namespace Thea {
24 namespace Math {
25 
26 namespace Internal {
27 
28 template <typename MatrixT, typename Enable = void>
29 struct IsSquareImpl
30 {
31  // Default implementation
32  static bool isSquare(MatrixT const & m)
33  {
34  return m.rows() == m.cols();
35  }
36 };
37 
38 } // namespace Internal
39 
41 template <typename MatrixT>
42 bool
43 isSquare(MatrixT const & m)
44 {
45  return Internal::IsSquareImpl<MatrixT>::isSquare(m);
46 }
47 
49 template <typename Derived, typename OutT>
50 void
51 getElementsRowMajor(Eigen::MatrixBase<Derived> const & m, OutT * buf)
52 {
53  alwaysAssertM(buf, "Math::getElementsRowMajor: Output buffer should be non-null");
54 
55  Eigen::Map< MatrixX<OutT, MatrixLayout::ROW_MAJOR> > bm(buf, m.rows(), m.cols());
56  bm = m.template cast<OutT>();
57 }
58 
60 template <typename Derived, typename OutT>
61 void
62 getElementsColumnMajor(Eigen::MatrixBase<Derived> const & m, OutT * buf)
63 {
64  alwaysAssertM(buf, "Math::getElementsColumnMajor: Output buffer should be non-null");
65 
66  Eigen::Map< MatrixX<OutT, MatrixLayout::COLUMN_MAJOR> > bm(buf, m.rows(), m.cols());
67  bm = m.template cast<OutT>();
68 }
69 
71 template <typename Derived>
72 Eigen::Index
73 minAxis(Eigen::MatrixBase<Derived> const & v)
74 {
75  Eigen::Index min_axis;
76  v.minCoeff(&min_axis);
77  return min_axis;
78 }
79 
81 template <typename Derived>
82 Eigen::Index
83 maxAxis(Eigen::MatrixBase<Derived> const & v)
84 {
85  Eigen::Index max_axis;
86  v.maxCoeff(&max_axis);
87  return max_axis;
88 }
89 
94 template <typename Derived>
95 Eigen::Index
96 minAbsAxis(Eigen::MatrixBase<Derived> const & v)
97 {
98  Eigen::Index min_axis;
99  v.cwiseAbs().minCoeff(&min_axis);
100  return min_axis;
101 }
102 
107 template <typename Derived>
108 Eigen::Index
109 maxAbsAxis(Eigen::MatrixBase<Derived> const & v)
110 {
111  Eigen::Index max_axis;
112  v.cwiseAbs().maxCoeff(&max_axis);
113  return max_axis;
114 }
115 
120 template <typename T, int N, int O1, int R1, int C1, int O2, int R2, int C2>
121 Eigen::Matrix<T, N - 1, 1, O2, R2, C2>
122 hmul(Eigen::Matrix<T, N, N, O1, R1, C1> const & m,
123  Eigen::Matrix<T, N - 1, 1, O2, R2, C2> const & v)
124 {
125  return (m * v.homogeneous()).hnormalized();
126 }
127 
132 template <int N, typename T = Real> Vector<N, T> oneHot(int coord) { return Vector<N, T>::Unit(coord); }
133 
138 template <typename Derived>
139 void
140 oneHot(int coord, Eigen::MatrixBase<Derived> & v)
141 {
142  static_assert(Derived::IsVectorAtCompileTime != 0, "Math::oneHot: Output must be compile-time vector");
143  debugAssertM(coord >= 0 && coord < v.size(), "Math::oneHot: Coordinate index out of bounds");
144 
145  v.fill(0); v[coord] = 1;
146 }
147 
152 template <int N, typename T = Real>
154 oneCold(int coord)
155 {
156  debugAssertM(coord >= 0 && coord < N, "Math::oneCold: Coordinate index out of bounds");
157 
158  Vector<N, T> v;
159  v.fill(1); v[coord] = 0;
160  return v;
161 }
162 
167 template <typename Derived>
168 void
169 oneCold(int coord, Eigen::MatrixBase<Derived> & v)
170 {
171  static_assert(Derived::IsVectorAtCompileTime != 0, "Math::oneCold: Output must be compile-time vector");
172  debugAssertM(coord >= 0 && coord < v.size(), "Math::oneCold: Coordinate index out of bounds");
173 
174  v.fill(1); v[coord] = 0;
175 }
176 
181 template <typename T>
184 {
185  return Vector<2, T>(v[1], -v[0]);
186 }
187 
193 template <typename T>
196 {
197  return orthogonalVector(v).normalized();
198 }
199 
201 template <typename T>
204 {
205  if (maxAbsAxis(v) == 0)
206  return Vector<3, T>(v[1], -v[0], 0).normalized();
207  else
208  return Vector<3, T>(0, v[2], -v[1]).normalized();
209 }
210 
217 template <typename T>
220 {
221  Vector<3, T> wnrm = w.normalized();
223  Vector<3, T> v = wnrm.cross(u);
224 
225  Matrix<3, 3, T> m; m << u, v, wnrm;
226  return m;
227 }
228 
235 template <typename T>
237 orthonormalBasis(Vector<3, T> const & u, Vector<3, T> const & v, Vector<3, T> const & w)
238 {
239  Vector<3, T> w2 = w.normalized();
240  Vector<3, T> u2 = v.cross(w2).normalized();
241  Vector<3, T> v2 = w2.cross(u2);
242 
243  Matrix<3, 3, T> m; m << u2, v2, w2;
244  return m;
245 }
246 
254 template <typename T>
257 {
258  return orthonormalBasis(Vector<3, T>(m.col(0)), Vector<3, T>(m.col(1)), Vector<3, T>(m.col(2)));
259 }
260 
266 template <int N, typename T>
269 {
270  return Matrix<N, N, T>(s.asDiagonal());
271 }
272 
274 template < int N, typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
276 scaling(T const & s)
277 {
278  Vector<N, T> v; v.fill(s);
279  return scaling(v);
280 }
281 
283 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
285 rotation(T const & radians)
286 {
287  T s = std::sin(radians);
288  T c = std::cos(radians);
289 
290  Matrix<2, 2, T> m; m << c, -s,
291  s, c;
292  return m;
293 }
294 
296 template <typename T>
298 rotationAxisAngle(Vector<3, T> const & axis, T const & radians)
299 {
300  Vector<3, T> uaxis = axis.normalized();
301 
302  T cos_val = std::cos(radians);
303  T sin_val = std::sin(radians);
304  T one_minus_cos = 1 - cos_val;
305  T x2 = uaxis[0] * uaxis[0];
306  T y2 = uaxis[1] * uaxis[1];
307  T z2 = uaxis[2] * uaxis[2];
308  T xym = uaxis[0] * uaxis[1] * one_minus_cos;
309  T xzm = uaxis[0] * uaxis[2] * one_minus_cos;
310  T yzm = uaxis[1] * uaxis[2] * one_minus_cos;
311  T xsin = uaxis[0] * sin_val;
312  T ysin = uaxis[1] * sin_val;
313  T zsin = uaxis[2] * sin_val;
314 
315  Matrix<3, 3, T> m;
316  m(0, 0) = x2 * one_minus_cos + cos_val;
317  m(0, 1) = xym - zsin;
318  m(0, 2) = xzm + ysin;
319 
320  m(1, 0) = xym + zsin;
321  m(1, 1) = y2 * one_minus_cos + cos_val;
322  m(1, 2) = yzm - xsin;
323 
324  m(2, 0) = xzm - ysin;
325  m(2, 1) = yzm + xsin;
326  m(2, 2) = z2 * one_minus_cos + cos_val;
327 
328  return m;
329 }
330 
334 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
336 rotationEulerAnglesXYZ(T const & yaw_radians, T const & pitch_radians, T const & roll_radians)
337 {
338  T cos_val, sin_val;
339 
340  cos_val = std::cos(yaw_radians);
341  sin_val = std::sin(yaw_radians);
342  Matrix<3, 3, T> xmat; xmat << 1, 0, 0, 0, cos_val, -sin_val, 0.0, sin_val, cos_val;
343 
344  cos_val = std::cos(pitch_radians);
345  sin_val = std::sin(pitch_radians);
346  Matrix<3, 3, T> ymat; ymat << cos_val, 0, sin_val, 0, 1, 0, -sin_val, 0, cos_val;
347 
348  cos_val = std::cos(roll_radians);
349  sin_val = std::sin(roll_radians);
350  Matrix<3, 3, T> zmat; zmat << cos_val, -sin_val, 0, sin_val, cos_val, 0, 0, 0, 1;
351 
352  return xmat * (ymat * zmat);
353 }
354 
358 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
360 rotationEulerAnglesXZY(T const & yaw_radians, T const & pitch_radians, T const & roll_radians)
361 {
362  T cos_val, sin_val;
363 
364  cos_val = std::cos(yaw_radians);
365  sin_val = std::sin(yaw_radians);
366  Matrix<3, 3, T> xmat; xmat << 1, 0, 0, 0, cos_val, -sin_val, 0, sin_val, cos_val;
367 
368  cos_val = std::cos(pitch_radians);
369  sin_val = std::sin(pitch_radians);
370  Matrix<3, 3, T> zmat; zmat << cos_val, -sin_val, 0, sin_val, cos_val, 0, 0, 0, 1;
371 
372  cos_val = std::cos(roll_radians);
373  sin_val = std::sin(roll_radians);
374  Matrix<3, 3, T> ymat; ymat << cos_val, 0, sin_val, 0, 1, 0, -sin_val, 0, cos_val;
375 
376  return xmat * (zmat * ymat);
377 }
378 
382 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
384 rotationEulerAnglesYXZ(T const & yaw_radians, T const & pitch_radians, T const & roll_radians)
385 {
386  T cos_val, sin_val;
387 
388  cos_val = std::cos(yaw_radians);
389  sin_val = std::sin(yaw_radians);
390  Matrix<3, 3, T> ymat; ymat << cos_val, 0, sin_val, 0, 1, 0, -sin_val, 0, cos_val;
391 
392  cos_val = std::cos(pitch_radians);
393  sin_val = std::sin(pitch_radians);
394  Matrix<3, 3, T> xmat; xmat << 1, 0, 0, 0, cos_val, -sin_val, 0, sin_val, cos_val;
395 
396  cos_val = std::cos(roll_radians);
397  sin_val = std::sin(roll_radians);
398  Matrix<3, 3, T> zmat; zmat << cos_val, -sin_val, 0, sin_val, cos_val, 0, 0, 0, 1;
399 
400  return ymat * (xmat * zmat);
401 }
402 
406 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
408 rotationEulerAnglesYZX(T const & yaw_radians, T const & pitch_radians, T const & roll_radians)
409 {
410  T cos_val, sin_val;
411 
412  cos_val = std::cos(yaw_radians);
413  sin_val = std::sin(yaw_radians);
414  Matrix<3, 3, T> ymat; ymat << cos_val, 0, sin_val, 0, 1, 0, -sin_val, 0, cos_val;
415 
416  cos_val = std::cos(pitch_radians);
417  sin_val = std::sin(pitch_radians);
418  Matrix<3, 3, T> zmat; zmat << cos_val, -sin_val, 0, sin_val, cos_val, 0, 0, 0, 1;
419 
420  cos_val = std::cos(roll_radians);
421  sin_val = std::sin(roll_radians);
422  Matrix<3, 3, T> xmat; xmat << 1, 0, 0, 0, cos_val, -sin_val, 0, sin_val, cos_val;
423 
424  return ymat * (zmat * xmat);
425 }
426 
430 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
432 rotationEulerAnglesZXY(T const & yaw_radians, T const & pitch_radians, T const & roll_radians)
433 {
434  T cos_val, sin_val;
435 
436  cos_val = std::cos(yaw_radians);
437  sin_val = std::sin(yaw_radians);
438  Matrix<3, 3, T> zmat; zmat << cos_val, -sin_val, 0, sin_val, cos_val, 0, 0, 0, 1;
439 
440  cos_val = std::cos(pitch_radians);
441  sin_val = std::sin(pitch_radians);
442  Matrix<3, 3, T> xmat; xmat << 1, 0, 0, 0, cos_val, -sin_val, 0, sin_val, cos_val;
443 
444  cos_val = std::cos(roll_radians);
445  sin_val = std::sin(roll_radians);
446  Matrix<3, 3, T> ymat; ymat << cos_val, 0, sin_val, 0, 1, 0, -sin_val, 0, cos_val;
447 
448  return zmat * (xmat * ymat);
449 }
450 
454 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
456 rotationEulerAnglesZYX(T const & yaw_radians, T const & pitch_radians, T const & roll_radians)
457 {
458  T cos_val, sin_val;
459 
460  cos_val = std::cos(yaw_radians);
461  sin_val = std::sin(yaw_radians);
462  Matrix<3, 3, T> zmat; zmat << cos_val, -sin_val, 0, sin_val, cos_val, 0, 0, 0, 1;
463 
464  cos_val = std::cos(pitch_radians);
465  sin_val = std::sin(pitch_radians);
466  Matrix<3, 3, T> ymat; ymat << cos_val, 0, sin_val, 0, 1, 0, -sin_val, 0, cos_val;
467 
468  cos_val = std::cos(roll_radians);
469  sin_val = std::sin(roll_radians);
470  Matrix<3, 3, T> xmat; xmat << 1, 0, 0, 0, cos_val, -sin_val, 0, sin_val, cos_val;
471 
472  return zmat * (ymat * xmat);
473 }
474 
487 template <typename T>
489 rotationArcQuat(Vector<3, T> const & start_dir, Vector<3, T> const & end_dir, bool normalize_dirs = true)
490 {
491  // From John Ratcliff's Code Suppository.
492  //
493  // Reference, from Stan Melax in Game Gems I
494  // Quaternion q;
495  // vector3 c = CrossProduct(v0,v1);
496  // REAL d = DotProduct(v0,v1);
497  // REAL s = (REAL)sqrt((1+d)*2);
498  // q.x = c.x / s;
499  // q.y = c.y / s;
500  // q.z = c.z / s;
501  // q.w = s / 2.0f;
502  // return q;
503 
504  Vector<3, T> u = start_dir, v = end_dir;
505  if (normalize_dirs)
506  {
507  u.normalize();
508  v.normalize();
509  }
510 
511  T d = u.dot(v);
512  if (d < -0.9999)
513  {
514  Vector<3, T> perp;
515  switch (maxAbsAxis(u))
516  {
517  case 0: perp = Vector<3, T>(u.y(), -u.x(), 0).normalized(); break;
518  case 1: perp = Vector<3, T>(u.y(), -u.x(), 0).normalized(); break;
519  default /* 2 */: perp = Vector<3, T>(u.z(), 0, -u.x()).normalized();
520  }
521 
522  return Quaternion<T>(Eigen::AngleAxis<T>(static_cast<T>(Math::pi()), perp));
523  }
524 
525  T s = std::sqrt((1 + d) * 2);
526  T recip = 1 / s;
527  Vector<3, T> rcc = recip * u.cross(v);
528 
529  return Quaternion<T>(static_cast<T>(0.5 * s), rcc.x(), rcc.y(), rcc.z());
530 }
531 
540 template <typename T>
542 rotationArc(Vector<3, T> const & start_dir, Vector<3, T> const & end_dir, bool normalize_dirs = true)
543 {
544  return rotationArcQuat(start_dir, end_dir, normalize_dirs).toRotationMatrix();
545 }
546 
552 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
554 orthogonalProjection(T const & left, T const & right, T const & bottom, T const & top, T const & nearval, T const & farval,
555  bool y_increases_upwards = true)
556 {
557  // Adapted from Mesa.
558  // Note that Microsoft (http://msdn.microsoft.com/library/default.asp?url=/library/en-us/opengl/glfunc03_8qnj.asp) and
559  // Linux (http://www.xfree86.org/current/glOrtho.3.html) have different matrices shown in their documentation.
560 
561  T x = 2 / (right - left);
562  T y = 2 / (top - bottom);
563  T z = -2 / (farval - nearval);
564  T tx = -(right + left) / (right - left);
565  T ty = -(top + bottom) / (top - bottom);
566  T tz = -(farval + nearval) / (farval - nearval);
567 
568  if (!y_increases_upwards)
569  {
570  y *= -1;
571  ty *= -1;
572  }
573 
574  return (Matrix<4, 4, T>() << x, 0, 0, tx,
575  0, y, 0, ty,
576  0, 0, z, tz,
577  0, 0, 0, 1).finished();
578 }
579 
585 template < typename T, typename std::enable_if< !std::is_integral<T>::value >::type * = nullptr >
587 perspectiveProjection(T const & left, T const & right, T const & bottom, T const & top, T const & nearval, T const & farval,
588  bool y_increases_upwards = true)
589 {
590  T x = (2 * nearval) / (right - left);
591  T y = (2 * nearval) / (top - bottom);
592  T a = (right + left) / (right - left);
593  T b = (top + bottom) / (top - bottom);
594 
595  T c, d;
596  if (Math::isInfinite(farval))
597  {
598  // Infinite view frustum
599  c = -1;
600  d = -2 * nearval;
601  }
602  else
603  {
604  c = -(farval + nearval) / (farval - nearval);
605  d = -(2 * farval * nearval) / (farval - nearval);
606  }
607 
608  if (!y_increases_upwards)
609  {
610  y *= -1;
611  b *= -1;
612  }
613 
614  return (Matrix<4, 4, T>() << x, 0, a, 0,
615  0, y, b, 0,
616  0, 0, c, d,
617  0, 0, -1, 0).finished();
618 }
619 
630 template <typename T>
631 int
632 eigenSolve(Matrix<2, 2, T> const & m, T * eigenvalues, Vector<2, T> * eigenvectors, T const & tol = -1)
633 {
634  T a = m(0, 0), b = m(0, 1);
635  T c = m(1, 0), d = m(1, 1);
636 
637  T trace = a + d;
638  T det = a * d - b * c;
639 
640  T disc = trace * trace / 4 - det;
641  if (disc < 0)
642  return 0;
643 
644  T s = std::sqrt(disc);
645  eigenvalues[0] = trace / 2 - s;
646  eigenvalues[1] = trace / 2 + s;
647 
648  if (!Math::fuzzyEq(c, static_cast<T>(0), (tol >= 0 ? tol : Math::eps(c, static_cast<T>(0)))))
649  {
650  eigenvectors[0][0] = eigenvalues[0] - d;
651  eigenvectors[0][1] = c;
652 
653  eigenvectors[1][0] = eigenvalues[1] - d;
654  eigenvectors[1][1] = c;
655  }
656  else if (!Math::fuzzyEq(b, static_cast<T>(0), (tol >= 0 ? tol : Math::eps(b, static_cast<T>(0)))))
657  {
658  eigenvectors[0][0] = b;
659  eigenvectors[0][1] = eigenvalues[0] - a;
660 
661  eigenvectors[1][0] = b;
662  eigenvectors[1][1] = eigenvalues[1] - a;
663  }
664  else
665  {
666  eigenvectors[0][0] = 1;
667  eigenvectors[0][1] = 0;
668 
669  eigenvectors[1][0] = 0;
670  eigenvectors[1][1] = 1;
671  }
672 
673  return 2;
674 }
675 
676 } // namespace Math
677 
699 template <typename Derived>
700 std::string
701 toString(Eigen::DenseBase<Derived> const & m, intx max_rows = 4, intx max_cols = 4)
702 {
703  intx first_rows = m.rows(), last_rows = 0;
704  intx first_cols = m.cols(), last_cols = 0;
705  if (m.rows() > max_rows)
706  {
707  first_rows = max_rows / 2 + max_rows % 2;
708  last_rows = max_rows / 2;
709  }
710  if (m.cols() > max_cols)
711  {
712  first_cols = max_cols / 2 + max_cols % 2;
713  last_cols = max_cols / 2;
714  }
715 
716  std::ostringstream oss;
717  oss << '[';
718 
719  for (int i = 0; i < 2; ++i)
720  {
721  intx row_begin = (i == 0 ? 0 : m.rows() - last_rows);
722  intx row_end = (i == 0 ? first_rows : m.rows());
723 
724  for (intx r = row_begin; r < row_end; ++r)
725  {
726  for (int j = 0; j < 2; ++j)
727  {
728  intx col_begin = (j == 0 ? 0 : m.cols() - last_cols);
729  intx col_end = (j == 0 ? first_cols : m.cols());
730 
731  for (intx c = col_begin; c < col_end; ++c)
732  {
733  if (c > col_begin) oss << ' ';
734  oss << m(r, c);
735  }
736 
737  if (j == 0 && (first_cols + last_cols) < m.cols())
738  oss << " ... ";
739  }
740 
741  if (r != m.rows() - 1) oss << "; ";
742  }
743 
744  if (i == 0)
745  {
746  if ((first_rows + last_rows) < m.rows()) oss << " ... ";
747  if (last_rows > 0) oss << "; ";
748  }
749  }
750 
751  oss << ']';
752  return oss.str();
753 }
754 
755 } // namespace Thea
756 
757 #endif
Eigen::Matrix< T, Size, 1, Options|((Options &Eigen::DontAlign)==0 &&Size==Eigen::Dynamic?Eigen::AutoAlign:Eigen::DontAlign), MaxRowsAtCompileTime, 1 > Vector
General 1D dense column vector template, alias for Eigen::Matrix<T, Size, 1,...>, with a custom align...
Definition: MatVec.hpp:146
void getElementsColumnMajor(Eigen::MatrixBase< Derived > const &m, OutT *buf)
Get the elements of a matrix in column-major order.
Definition: MatrixUtil.hpp:62
Eigen::Index minAbsAxis(Eigen::MatrixBase< Derived > const &v)
Get the coordinate of a vector with the least absolute value.
Definition: MatrixUtil.hpp:96
Eigen::Index minAxis(Eigen::MatrixBase< Derived > const &v)
Get the coordinate of a vector with the least value.
Definition: MatrixUtil.hpp:73
Matrix< 3, 3, T > rotationEulerAnglesZXY(T const &yaw_radians, T const &pitch_radians, T const &roll_radians)
Rotate about the Y axis by the roll angle, then the X axis by the pitch angle, and finally the Z axis...
Definition: MatrixUtil.hpp:432
std::ptrdiff_t intx
A signed integer suitable for indexing a structure held in memory.
Definition: Platform.hpp:161
std::string toString(char const *s)
Convert a C-string to a std::string object, returning an empty string if the input is a null pointer...
Eigen::Quaternion< T > Quaternion
Alias for Eigen::Quaternion<...>.
Definition: MatVec.hpp:359
Root namespace for the Thea library.
Vector< 2, T > orthogonalDirection(Vector< 2, T > const &v)
Given a 2D vector v, get the unit vector u perpendicular to it, forming an orthonormal right-handed b...
Definition: MatrixUtil.hpp:195
Matrix< 4, 4, T > perspectiveProjection(T const &left, T const &right, T const &bottom, T const &top, T const &nearval, T const &farval, bool y_increases_upwards=true)
Constructs a 3D perspective projection matrix from the given parameters.
Definition: MatrixUtil.hpp:587
T eps()
An "epsilon" threshold for comparing a number to zero.
Definition: Math.hpp:189
Matrix< 2, 2, T > rotation(T const &radians)
Matrix to rotate a 2D vector about the origin by an angle (in radians).
Definition: MatrixUtil.hpp:285
Eigen::Matrix< T, N-1, 1, O2, R2, C2 > hmul(Eigen::Matrix< T, N, N, O1, R1, C1 > const &m, Eigen::Matrix< T, N-1, 1, O2, R2, C2 > const &v)
Convenience function to multiply a 4x4 matrix by a 3-vector, by converting to homogeneous coordinates...
Definition: MatrixUtil.hpp:122
bool isInfinite(T const &t)
Check if a number represents (positive or negative) infinity.
Definition: Math.hpp:173
Eigen::Index maxAbsAxis(Eigen::MatrixBase< Derived > const &v)
Get the coordinate of a vector with the largest absolute value.
Definition: MatrixUtil.hpp:109
Matrix< N, N, T > scaling(Vector< N, T > const &s)
Matrix to scale a point by the corresponding scaling parameter along each dimension.
Definition: MatrixUtil.hpp:268
Vector< N, T > oneCold(int coord)
Get the one-cold vector with all entries 1 except a single entry which is 0.
Definition: MatrixUtil.hpp:154
Vector< 2, T > orthogonalVector(Vector< 2, T > const &v)
Given a 2D vector v, get the vector u perpendicular to it and of the same length, forming a right-han...
Definition: MatrixUtil.hpp:183
double pi()
The constant Pi.
Definition: Math.hpp:292
Eigen::Index maxAxis(Eigen::MatrixBase< Derived > const &v)
Get the coordinate of a vector with the largest value.
Definition: MatrixUtil.hpp:83
int eigenSolve(Matrix< 2, 2, T > const &m, T *eigenvalues, Vector< 2, T > *eigenvectors, T const &tol=-1)
Solve for the real eigenvalues and eigenvectors of a 2x2 matrix.
Definition: MatrixUtil.hpp:632
Quaternion< T > rotationArcQuat(Vector< 3, T > const &start_dir, Vector< 3, T > const &end_dir, bool normalize_dirs=true)
Return the quaternion corresponding to the rotation from one direction vector to another.
Definition: MatrixUtil.hpp:489
bool fuzzyEq(T const &a, T const &b, T const &tol)
Check if two numbers are approximately equal, with a given tolerance.
Definition: Math.hpp:210
Matrix< 3, 3, T > rotationEulerAnglesYXZ(T const &yaw_radians, T const &pitch_radians, T const &roll_radians)
Rotate about the Z axis by the roll angle, then the X axis by the pitch angle, and finally the Y axis...
Definition: MatrixUtil.hpp:384
Matrix< 3, 3, T > rotationAxisAngle(Vector< 3, T > const &axis, T const &radians)
Rotate around the given 3D axis (need not be a unit vector) by a given angle.
Definition: MatrixUtil.hpp:298
Vector< N, T > oneHot(int coord)
Get the one-hot vector with all entries 0 except a single entry which is 1.
Definition: MatrixUtil.hpp:132
Matrix< 4, 4, T > orthogonalProjection(T const &left, T const &right, T const &bottom, T const &top, T const &nearval, T const &farval, bool y_increases_upwards=true)
Constructs an orthogonal 3D projection matrix from the given parameters.
Definition: MatrixUtil.hpp:554
Matrix< 3, 3, T > rotationEulerAnglesZYX(T const &yaw_radians, T const &pitch_radians, T const &roll_radians)
Rotate about the X axis by the roll angle, then the Y axis by the pitch angle, and finally the Z axis...
Definition: MatrixUtil.hpp:456
Matrix< 3, 3, T > rotationArc(Vector< 3, T > const &start_dir, Vector< 3, T > const &end_dir, bool normalize_dirs=true)
Return the matrix corresponding to the rotation from one direction vector to another.
Definition: MatrixUtil.hpp:542
Matrix< 3, 3, T > rotationEulerAnglesYZX(T const &yaw_radians, T const &pitch_radians, T const &roll_radians)
Rotate about the X axis by the roll angle, then the Z axis by the pitch angle, and finally the Y axis...
Definition: MatrixUtil.hpp:408
Matrix< 3, 3, T > orthonormalBasis(Vector< 3, T > const &w)
Given a 3D vector w (need not be unit length), construct a 3x3 rotation matrix whose last column is t...
Definition: MatrixUtil.hpp:219
bool isSquare(MatrixT const &m)
Check if a matrix is square, that is, has the same number of rows and columns.
Definition: MatrixUtil.hpp:43
Eigen::Matrix< T, Rows, Cols, Options|((Options &Eigen::DontAlign)==0 &&(Rows==Eigen::Dynamic||Cols==Eigen::Dynamic)?Eigen::AutoAlign:Eigen::DontAlign), MaxRowsAtCompileTime, MaxColsAtCompileTime > Matrix
General 2D dense matrix template, alias for Eigen::Matrix with a custom default layout (row or column...
Definition: MatVec.hpp:114
void alwaysAssertM(CondT const &test, MessageT const &msg)
Check if a test condition is true, and immediately abort the program with an error code if not...
Definition: Common.hpp:66
void getElementsRowMajor(Eigen::MatrixBase< Derived > const &m, OutT *buf)
Get the elements of a matrix in row-major order.
Definition: MatrixUtil.hpp:51
Matrix< 3, 3, T > rotationEulerAnglesXZY(T const &yaw_radians, T const &pitch_radians, T const &roll_radians)
Rotate about the Y axis by the roll angle, then the Z axis by the pitch angle, and finally the X axis...
Definition: MatrixUtil.hpp:360
Matrix< 3, 3, T > rotationEulerAnglesXYZ(T const &yaw_radians, T const &pitch_radians, T const &roll_radians)
Rotate about the Z axis by the roll angle, then the Y axis by the pitch angle, and finally the X axis...
Definition: MatrixUtil.hpp:336
void debugAssertM(CondT const &test, MessageT const &msg)
Check if a test condition is true, and immediately abort the program with an error code if not...
Definition: Common.hpp:52