LinearAlgebra.h
Go to the documentation of this file.
1 // (C) Copyright Renaud Detry 2007-2015.
2 // Distributed under the GNU General Public License and under the
3 // BSD 3-Clause License (See accompanying file LICENSE.txt).
4 
5 /** @file */
6 
7 #ifndef NUKLEI_LINEAR_ALGEBRA_H
8 #define NUKLEI_LINEAR_ALGEBRA_H
9 
10 #include <sstream>
11 #include <nuklei/Random.h>
12 #include <nuklei/BoostSerialization.h>
13 #include <nuklei/Common.h>
14 #include <nuklei/Math.h>
15 
16 namespace nuklei {
17 
18  /** @brief Namespace containing linear algebra functions (and some other functions). */
19  namespace la {
20 
21  inline
22  Quaternion quaternionCopy(const Matrix3& m)
23  {
24  Quaternion q;
25  q.FromRotationMatrix(m);
26  q.Normalize();
27  return q;
28  }
29 
30  inline
31  Matrix3 matrixCopy(const Quaternion &q)
32  {
33  Matrix3 m;
34  q.ToRotationMatrix(m);
35  m.Orthonormalize();
36  return m;
37  }
38 
39 
40  inline
41  Quaternion quaternionCopy(const Quaternion& q)
42  {
43  return q;
44  }
45 
46  inline
47  Matrix3 matrixCopy(const Matrix3 &m)
48  {
49  return m;
50  }
51 
52 
53  inline
54  void copyRotation(Quaternion& q, const Matrix3& m)
55  {
56  q = quaternionCopy(m);
57  }
58 
59  inline
60  void copyRotation(Matrix3& m, const Quaternion& q)
61  {
62  m = matrixCopy(q);
63  }
64 
65  inline
66  void copyRotation(Quaternion& q, const Quaternion& q2)
67  {
68  q = q2;
69  }
70 
71  inline
72  void copyRotation(Matrix3& m, const Matrix3& m2)
73  {
74  m = m2;
75  }
76 
77  inline
78  GVector gVectorCopy(const GVector& v)
79  {
80  return v;
81  }
82 
83  inline
84  GVector gVectorCopy(const Vector3& v)
85  {
86  return GVector(3, v);
87  }
88 
89  inline
90  Vector3 vector3Copy(const Vector3& v)
91  {
92  return v;
93  }
94 
95  inline
96  Vector3 vector3Copy(const GVector& v)
97  {
98  NUKLEI_ASSERT(v.GetSize() == 3)
99  return Vector3(v[0], v[1], v[2]);
100  }
101  }
102 
103  inline
104  std::istream& operator>>(std::istream &in, Vector3 &l)
105  {
106  NUKLEI_ASSERT(in >> l.X() >> l.Y() >> l.Z());
107  return in;
108  }
109 
110 
111  inline
112  std::istream& operator>>(std::istream &in, Quaternion &q)
113  {
114  NUKLEI_ASSERT(in >> q.W() >> q.X() >> q.Y() >> q.Z());
115  return in;
116  }
117 
118  inline
119  std::ostream& operator<<(std::ostream &out, const Quaternion &q)
120  {
121  NUKLEI_ASSERT(out << q.W() << ' ' << q.X() << ' ' << q.Y() << ' ' << q.Z());
122  return out;
123  }
124 
125  inline
126  std::ostream& operator<<(std::ostream &out, const Matrix3 &m)
127  {
128  NUKLEI_ASSERT(out << la::quaternionCopy(m));
129  return out;
130  }
131 
132  inline
133  std::istream& operator>>(std::istream &in, Matrix3 &m)
134  {
135  Quaternion q;
136  NUKLEI_ASSERT(in >> q);
137  m = la::matrixCopy(q);
138  return in;
139  }
140 
141  std::ostream& operator<<(std::ostream &out, const GMatrix &m);
142  std::istream& operator>>(std::istream &in, GMatrix &m);
143 
144 }
145 
146 
147 namespace nuklei {
148 
149  namespace la {
150 
151 
152  inline
153  Vector3 normalized(const Vector3 &v)
154  {
155  Vector3 unit = v;
156  unit.Normalize();
157  return unit;
158  }
159 
160  inline
161  Quaternion normalized(const Quaternion &q)
162  {
163  Quaternion n = q;
164  n.Normalize();
165  return n;
166  }
167 
168  inline
169  Matrix3 normalized(const Matrix3 &m)
170  {
171  Matrix3 n = m;
172  n.Orthonormalize();
173  return n;
174  }
175 
176  inline
177  void makeZero(Vector3 &v)
178  {
179  v = Vector3::ZERO;
180  }
181 
182  inline
183  void makeIdentity(Matrix3 &m)
184  {
185  m = Matrix3::IDENTITY;
186  }
187 
188  inline
189  void makeIdentity(Quaternion &q)
190  {
191  q = Quaternion::IDENTITY;
192  }
193 
194 
195  inline
196  Vector3 min(const Vector3 &v1, const Vector3 &v2)
197  {
198  Vector3 minv;
199  for (int i = 0; i < 3; ++i)
200  minv[i] = std::min(v1[i], v2[i]);
201  return minv;
202  }
203 
204 
205  inline
206  Vector3 max(const Vector3 &v1, const Vector3 &v2)
207  {
208  Vector3 maxv;
209  for (int i = 0; i < 3; ++i)
210  maxv[i] = std::max(v1[i], v2[i]);
211  return maxv;
212  }
213 
214  inline
215  Vector3 mean(const Vector3 &v1, const Vector3 &v2)
216  {
217  Vector3 mean;
218  for (int i = 0; i < 3; ++i)
219  mean[i] = ( v1[i] + v2[i] ) / 2.0;
220  return mean;
221  }
222 
223 
224 
225  inline
226  Vector3 xAxis(const Quaternion &q)
227  {
228  typedef coord_t Real;
229 
230  Real fTy = ((Real)2.0)*q[2];
231  Real fTz = ((Real)2.0)*q[3];
232  Real fTwy = fTy*q[0];
233  Real fTwz = fTz*q[0];
234  Real fTxy = fTy*q[1];
235  Real fTxz = fTz*q[1];
236  Real fTyy = fTy*q[2];
237  Real fTzz = fTz*q[3];
238 
239  Vector3 x((Real)1.0-(fTyy+fTzz),
240  fTxy+fTwz,
241  fTxz-fTwy);
242 
243  return normalized(x);
244  }
245 
246  inline
247  Vector3 xAxis(const Matrix3 &m)
248  {
249  return m.GetColumn(0);
250  }
251 
252 
253  inline
254  coord_t numDrift(const Quaternion &q)
255  {
256  return std::fabs(q.Length()-1);
257  }
258 
259  inline
260  coord_t numDrift(const Matrix3 &m)
261  {
262  coord_t sum = 1;
263  for (int i = 0; i < 3; ++i)
264  sum += std::fabs(Vector3(m.GetColumn(i)).Dot(m.GetColumn((i+1)%3))) + std::fabs(Vector3(m.GetColumn(i)).Length() - 1);
265 
266  return sum-1;
267  }
268 
269  inline
270  void check(const Quaternion &q, const char* msg)
271  {
272  coord_t nd = numDrift(q);
273  if (!(nd < FLOATTOL))
274  {
275  std::cout << msg << ": \n" << NUKLEI_NVP(q) << "\n" <<
276  NUKLEI_NVP(numDrift(q)) << std::endl;
277  }
278  NUKLEI_ASSERT(nd < FLOATTOL);
279  }
280 
281  inline
282  void check(const Matrix3 &m, const char* msg)
283  {
284  coord_t nd = numDrift(m);
285  if (!(nd < FLOATTOL))
286  {
287  std::cout << msg << ": \n" << NUKLEI_NVP(m) << "\n" <<
288  NUKLEI_NVP(numDrift(m)) << std::endl;
289  }
290  NUKLEI_ASSERT(nd < FLOATTOL);
291  }
292 
293  inline
294  Quaternion inverseRotation(const Quaternion &q)
295  {
296  return q.Conjugate();
297  }
298 
299  inline
300  Matrix3 inverseRotation(const Matrix3 &m)
301  {
302  return m.Transpose();
303  }
304 
305 
306  inline
307  Quaternion slerp(double c,
308  const Quaternion &q1,
309  const Quaternion &q2)
310  {
311  //fixme: check this slerp method
312  // if c == 0, s = m1 (check that)
313  Quaternion q;
314 
315  if (q1.Dot(q2) < 0)
316  q.Slerp(c, q1, -q2);
317  else
318  q.Slerp(c, q1, q2);
319 
320  q.Normalize();
321 
322  return q;
323  }
324 
325  inline
326  Matrix3 slerp(double c,
327  const Matrix3 &m1,
328  const Matrix3 &m2)
329  {
330  return matrixCopy(slerp(c, quaternionCopy(m1), quaternionCopy(m2)));
331  }
332 
333 
334  inline
335  Quaternion mean(const Quaternion &q1,
336  const Quaternion &q2)
337  {
338  return slerp(.5, q1, q2);
339  }
340 
341  inline
342  Matrix3 mean(const Matrix3 &m1,
343  const Matrix3 &m2)
344  {
345  return matrixCopy(mean(quaternionCopy(m1), quaternionCopy(m2)));
346  }
347 
348 
349  inline
350  Quaternion so3FromS2(const Vector3 &w)
351  {
352  Vector3 u, v;
353  Vector3::GenerateComplementBasis(u, v, w);
354 
355  Matrix3 m;
356  coord_t angle = 2*M_PI*Random::uniform();
357  Vector3 up = std::cos(angle)*u + std::sin(angle)*v;
358  Vector3 vp = -std::sin(angle)*u + std::cos(angle)*v;
359 
360  m.SetColumn(0, w);
361  m.SetColumn(1, up);
362  m.SetColumn(2, vp);
363 
364  return la::quaternionCopy(m);
365  }
366 
367  void eigenDecomposition(Matrix3 &eVectors, Vector3& eValues, const Matrix3& sym);
368 
369  double determinant(const GMatrix &m);
370  GMatrix inverse(const GMatrix &m);
371 
372  //Vector3 project(const Plane3& plane, const Vector3& point);
373 
374 
375  /**
376  * @addtogroup matrix_transfo
377  * @{
378  */
379 
380 
381  /** @brief Returns @f$ Xy + x @f$. */
382  inline Vector3 transform(const Vector3& x,
383  const Matrix3& X,
384  const Vector3& y)
385  {
386  return X*y + x;
387  }
388 
389  /** @brief Returns @f$ Xy + x @f$. */
390  inline Vector3 transform(const Vector3& x,
391  const Quaternion& X,
392  const Vector3& y)
393  {
394  return X.Rotate(y) + x;
395  }
396 
397  /** @brief @f$ z = X y + x, Z = X Y @f$. */
398  inline void transform(Vector3& z, Matrix3& Z,
399  const Vector3& x, const Matrix3& X,
400  const Vector3& y, const Matrix3& Y)
401  {
402  z = transform(x, X, y);
403  Z = X*Y;
404  }
405 
406  /** @brief @f$ z = X y + x, Z = X Y @f$. */
407  inline void transform(Vector3& z, Quaternion& Z,
408  const Vector3& x, const Quaternion& X,
409  const Vector3& y, const Quaternion& Y)
410  {
411  z = transform(x, X, y);
412  Z = X*Y;
413  }
414 
415  /** @brief @f$ z = X y + x, Z = X Y @f$. */
416  inline void transform(Vector3& z, Vector3& Z,
417  const Vector3& x, const Matrix3& X,
418  const Vector3& y, const Vector3& Y)
419  {
420  z = transform(x, X, y);
421  Z = transform(Vector3::ZERO, X, Y);
422  }
423 
424  /** @brief @f$ z = X y + x, Z = X Y @f$. */
425  inline void transform(Vector3& z, Vector3& Z,
426  const Vector3& x, const Quaternion& X,
427  const Vector3& y, const Vector3& Y)
428  {
429  z = transform(x, X, y);
430  Z = transform(Vector3::ZERO, X, Y);
431  }
432 
433  /** @brief Returns @f$ X^T (z-x) @f$. */
434  inline Vector3 project(const Vector3& x,
435  const Matrix3& X,
436  const Vector3& z)
437  {
438  return X.Transpose() * (z-x);
439  }
440 
441  /** @brief Returns @f$ X^T (z-x) @f$. */
442  inline Vector3 project(const Vector3& x,
443  const Quaternion& X,
444  const Vector3& z)
445  {
446  return X.Conjugate().Rotate(z-x);
447  }
448 
449  /** @brief @f$ y = X^T (z-x), Y = X^T Z @f$. */
450  inline void project(Vector3& y, Matrix3& Y,
451  const Vector3& x, const Matrix3& X,
452  const Vector3& z, const Matrix3& Z)
453  {
454  y = project(x, X, z);
455  Y = X.TransposeTimes(Z);
456  }
457 
458  /** @brief @f$ y = X^T (z-x), Y = X^T Z @f$. */
459  inline void project(Vector3& y, Quaternion& Y,
460  const Vector3& x, const Quaternion& X,
461  const Vector3& z, const Quaternion& Z)
462  {
463  y = project(x, X, z);
464  Y = X.Conjugate() * Z;
465  }
466 
467  /** @brief @f$ y = X^T (z-x), Y = X^T Z @f$. */
468  inline void project(Vector3& y, Vector3& Y,
469  const Vector3& x, const Matrix3& X,
470  const Vector3& z, const Vector3& Z)
471  {
472  y = project(x, X, z);
473  Y = project(Vector3::ZERO, X, Z);
474  }
475 
476  /** @brief @f$ y = X^T (z-x), Y = X^T Z @f$. */
477  inline void project(Vector3& y, Vector3& Y,
478  const Vector3& x, const Quaternion& X,
479  const Vector3& z, const Vector3& Z)
480  {
481  y = project(x, X, z);
482  Y = project(Vector3::ZERO, X, Z);
483  }
484 
485  /** @brief @f$ x = z - Z Y^T y, X = Z Y^T @f$ */
486  inline void transfoTo(Vector3& x, Matrix3& X,
487  const Vector3& y, const Matrix3& Y,
488  const Vector3& z, const Matrix3& Z)
489  {
490  X = Z.TimesTranspose(Y);
491  x = z - X*y;
492  }
493 
494  /** @brief @f$ x = z - Z Y^T y, X = Z Y^T @f$ */
495  inline void transfoTo(Vector3& x, Quaternion& X,
496  const Vector3& y, const Quaternion& Y,
497  const Vector3& z, const Quaternion& Z)
498  {
499  X = Z * Y.Conjugate();
500  x = z - X.Rotate(y);
501  }
502 
503  /** @} */
504 
505  template<class R> void fromAngleAxisString(R &r, const std::string &angleAxis)
506  {
507  coord_t angle;
508  Vector3 axis;
509  std::istringstream ris(angleAxis);
510  NUKLEI_ASSERT(ris >> angle);
511  NUKLEI_ASSERT(ris >> axis);
512  NUKLEI_ASSERT_AFE(axis.Length(), 1);
513  axis = normalized(axis);
514  r.FromAxisAngle(axis, angle);
515  r = normalized(r);
516  }
517 
518  template<class R> std::string toAngleAxisString(const R &r)
519  {
520  Vector3 axis;
521  coord_t angle;
522  r.ToAxisAngle(axis, angle);
523  return stringify(angle) + " " + stringify(axis);
524  }
525 
526  }
527 
528  inline coord_t
529  multivariateGaussian(const GVector &x, const GVector &m, const GMatrix &cov,
530  const weight_t w = 1)
531  {
532  int d = x.GetSize();
533  NUKLEI_FAST_DEBUG_ASSERT(d = m.GetSize());
534  const GVector diff = x-m;
535  const GMatrix inv = la::inverse(cov);
536  const GVector tmp = inv * diff;
537  double val = diff.Dot(tmp);
538  double ret = std::exp(-.5 * val);
539  ret /= std::pow(2*M_PI, double(d)/2) * std::sqrt(la::determinant(cov));
540  return w*ret;
541  }
542 
543 
544 }
545 
546 NUKLEI_SERIALIZATION_NAMESPACE_BEGIN
547 
548  template<class Archive>
549  void serialize(Archive & ar, nuklei_wmf::Vector3<nuklei::coord_t> &v, const unsigned int version)
550  {
551  ar & NUKLEI_SERIALIZATION_MAKE_NVP("x", v.X());
552  ar & NUKLEI_SERIALIZATION_MAKE_NVP("y", v.Y());
553  ar & NUKLEI_SERIALIZATION_MAKE_NVP("z", v.Z());
554  }
555 
556  template<class Archive>
557  void serialize(Archive & ar, nuklei_wmf::Quaternion<nuklei::coord_t> &q, const unsigned int version)
558  {
559  ar & NUKLEI_SERIALIZATION_MAKE_NVP("w", q.W());
560  ar & NUKLEI_SERIALIZATION_MAKE_NVP("x", q.X());
561  ar & NUKLEI_SERIALIZATION_MAKE_NVP("y", q.Y());
562  ar & NUKLEI_SERIALIZATION_MAKE_NVP("z", q.Z());
563  }
564 
565  template<class Archive>
566  void serialize(Archive & ar, nuklei_wmf::Matrix3<nuklei::coord_t> &m, const unsigned int version)
567  {
568  nuklei_wmf::Quaternion<nuklei::coord_t> q = nuklei::la::quaternionCopy(m);
569  serialize(ar, q, version);
570  nuklei::la::copyRotation(m, q);
571  }
572 
573 
574 NUKLEI_SERIALIZATION_NAMESPACE_END
575 
576 
577 
578 #endif
nuklei
Public namespace.
Definition: Color.cpp:9
nuklei::weight_t
double weight_t
Type for particle weights.
Definition: Definitions.h:31
nuklei::stringify
std::string stringify(const T &x, int precision=-1, int width=0)
Converts an object to a std::string using operator<<.
Definition: Common.h:333
nuklei::la::transform
Vector3 transform(const Vector3 &x, const Matrix3 &X, const Vector3 &y)
Returns .
Definition: LinearAlgebra.h:382
BoostSerialization.h
NUKLEI_ASSERT
#define NUKLEI_ASSERT(expression)
Throws an Error if expression is not true.
Definition: Common.h:113
Math.h
nuklei::coord_t
double coord_t
Type for point coordinates.
Definition: Definitions.h:25
NUKLEI_NVP
#define NUKLEI_NVP(x)
Name-value pair.
Definition: Common.h:76
Common.h
nuklei::Random::uniform
static double uniform()
This function returns a double precision floating point number uniformly distributed in the range .
Definition: Random.cpp:159
Random.h
nuklei::la::project
Vector3 project(const Vector3 &x, const Matrix3 &X, const Vector3 &z)
Returns .
Definition: LinearAlgebra.h:434
nuklei::la::transfoTo
void transfoTo(Vector3 &x, Matrix3 &X, const Vector3 &y, const Matrix3 &Y, const Vector3 &z, const Matrix3 &Z)
Definition: LinearAlgebra.h:486
© Copyright 2007-2013 Renaud Detry.
Distributed under the terms of the GNU General Public License (GPL).
(See accompanying file LICENSE.txt or copy at http://www.gnu.org/copyleft/gpl.html.)
Revised Sun Sep 13 2020 19:10:06.