quaternion.cpp
Go to the documentation of this file.
1 /*
2 Copyright (C) 2002-2004 Etienne Lachance
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
22 
23 -------------------------------------------------------------------------------
24 Revision_history:
25 
26 2004/01/19: Etienne Lachance
27  -Removed function Exp and Ln.
28  -Added function power and Log.
29  -Fixed bugs in Slerp, Slerp_prime, Squad and Squad_prime.
30 
31 2003/05/23: Etienne Lachance
32  -Added the following member function -=, +=, *=, /=, Exp, Ln, dot, d_dt, E
33  -Added functions Integ_Trap_quat, Slerp, Slerp_prime, Squad, Squad_prime,
34 
35 2004/05/14: Etienne Lachance
36  -Replaced vec_x_prod by CrossProduct.
37 
38 2004/05/21: Etienne Lachance
39  -Added comments that can be used with Doxygen.
40 
41 2004/07/01: Etienne Lachance
42  -Replaced vec_dot_prod by DotProdut of Newmat library.
43 
44 2004/07/01: Ethan Tira-Thompson
45  -Added support for newmat's use_namespace #define, using ROBOOP namespace
46  -Fixed problem in constructor using float as Real type
47 
48 2005/11/06: Etienne Lachance
49  - No need to provide a copy constructor and the assignment operator
50  (operator=) for Quaternion class. Instead we use the one provide by the
51  compiler.
52 
53 2005/11/13: Etienne Lachance
54  - operator* and operator/ are now non-member functions when one of the
55  operand is a real. With these modifications we support q2 = c * q1 and
56  q2 = q1 * c
57 -------------------------------------------------------------------------------
58 */
59 
60 
66 static const char rcsid[] = "$Id: quaternion.cpp,v 1.18 2005/11/15 19:25:58 gourdeau Exp $";
68 
69 #include "quaternion.h"
70 
71 #ifdef use_namespace
72 namespace ROBOOP {
73  using namespace NEWMAT;
74 #endif
75 
76 
79 {
80  s_ = 1.0;
81  v_ = ColumnVector(3);
82  v_ = 0.0;
83 }
84 
87 {
88  if(axis.Nrows() != 3)
89  {
90  cerr << "Quaternion::Quaternion, size of axis != 3" << endl;
91  exit(1);
92  }
93 
94  // make sure axis is a unit vector
95  Real norm_axis = sqrt(DotProduct(axis, axis));
96 
97  if(norm_axis != 1)
98  {
99  cerr << "Quaternion::Quaternion(angle, axis), axis is not unit" << endl;
100  cerr << "Make the axis unit." << endl;
101  v_ = sin(angle/2) * axis/norm_axis;
102  }
103  else
104  v_ = sin(angle/2) * axis;
105 
106  s_ = cos(angle/2);
107 }
108 
109 Quaternion::Quaternion(const Real s_in, const Real v1, const Real v2,
110  const Real v3)
112 {
113  s_ = s_in;
114  v_ = ColumnVector(3);
115  v_(1) = v1;
116  v_(2) = v2;
117  v_(3) = v3;
118 }
119 
161 {
162  if( (R.Nrows() == 3) && (R.Ncols() == 3) ||
163  (R.Nrows() == 4) && (R.Ncols() == 4) )
164  {
165  Real tmp = fabs(R(1,1) + R(2,2) + R(3,3) + 1);
166  s_ = 0.5*sqrt(tmp);
167  if(v_.Nrows() != 3)
168  v_ = ColumnVector(3);
169 
170  if(s_ > EPSILON)
171  {
172  v_(1) = (R(3,2)-R(2,3))/(4*s_);
173  v_(2) = (R(1,3)-R(3,1))/(4*s_);
174  v_(3) = (R(2,1)-R(1,2))/(4*s_);
175  }
176  else
177  {
178  // |w| <= 1/2
179  static int s_iNext[3] = { 2, 3, 1 };
180  int i = 1;
181  if ( R(2,2) > R(1,1) )
182  i = 2;
183  if ( R(3,3) > R(2,2) )
184  i = 3;
185  int j = s_iNext[i-1];
186  int k = s_iNext[j-1];
187 
188  Real fRoot = sqrt(R(i,i)-R(j,j)-R(k,k) + 1.0);
189 
190  Real *tmp[3] = { &v_(1), &v_(2), &v_(3) };
191  *tmp[i-1] = 0.5*fRoot;
192  fRoot = 0.5/fRoot;
193  s_ = (R(k,j)-R(j,k))*fRoot;
194  *tmp[j-1] = (R(j,i)+R(i,j))*fRoot;
195  *tmp[k-1] = (R(k,i)+R(i,k))*fRoot;
196  }
197 
198  }
199  else
200  cerr << "Quaternion::Quaternion: matrix input is not 3x3 or 4x4" << endl;
201 }
202 
215 {
216  Quaternion q;
217  q.s_ = s_ + rhs.s_;
218  q.v_ = v_ + rhs.v_;
219 
220  return q;
221 }
222 
235 {
236  Quaternion q;
237  q.s_ = s_ - rhs.s_;
238  q.v_ = v_ - rhs.v_;
239 
240  return q;
241 }
242 
258 {
259  Quaternion q;
260  q.s_ = s_ * rhs.s_ - DotProduct(v_, rhs.v_);
261  q.v_ = s_ * rhs.v_ + rhs.s_ * v_ + CrossProduct(v_, rhs.v_);
262 
263  return q;
264 }
265 
266 
269 {
270  return *this*rhs.i();
271 }
272 
273 
276 {
277  if(v.Nrows() == 3)
278  v_ = v;
279  else
280  cerr << "Quaternion::set_v: input has a wrong size." << endl;
281 }
282 
290 {
291  Quaternion q;
292  q.s_ = s_;
293  q.v_ = -1*v_;
294 
295  return q;
296 }
297 
307 {
308  return( sqrt(s_*s_ + DotProduct(v_, v_)) );
309 }
310 
313 {
314  Real tmp = norm();
315  if(tmp > EPSILON)
316  {
317  s_ = s_/tmp;
318  v_ = v_/tmp;
319  }
320  return *this;
321 }
322 
332 {
333  return conjugate()/norm();
334 }
335 
344 {
345  Quaternion q;
346  Real theta = sqrt(DotProduct(v_,v_)),
347  sin_theta = sin(theta);
348 
349  q.s_ = cos(theta);
350  if ( fabs(sin_theta) > EPSILON)
351  q.v_ = v_*sin_theta/theta;
352  else
353  q.v_ = v_;
354 
355  return q;
356 }
357 
359 {
360  Quaternion q = (Log()*t).exp();
361 
362  return q;
363 }
364 
374 {
375  Quaternion q;
376  q.s_ = 0;
377  Real theta = acos(s_),
378  sin_theta = sin(theta);
379 
380  if ( fabs(sin_theta) > EPSILON)
381  q.v_ = v_/sin_theta*theta;
382  else
383  q.v_ = v_;
384 
385  return q;
386 }
387 
388 Quaternion Quaternion::dot(const ColumnVector & w, const short sign)const
415 {
416  Quaternion q;
417  Matrix tmp;
418 
419  tmp = -0.5*v_.t()*w;
420  q.s_ = tmp(1,1);
421  q.v_ = 0.5*E(sign)*w;
422 
423  return q;
424 }
425 
426 ReturnMatrix Quaternion::E(const short sign)const
432 {
433  Matrix E(3,3), I(3,3);
434  I << threebythreeident;
435 
436  if(sign == BODY_FRAME)
437  E = s_*I + x_prod_matrix(v_);
438  else
439  E = s_*I - x_prod_matrix(v_);
440 
441  E.Release();
442  return E;
443 }
444 
454 {
455  return (s_*q.s_ + v_(1)*q.v_(1) + v_(2)*q.v_(2) + v_(3)*q.v_(3));
456 }
457 
490 {
491  Matrix R(3,3);
492  R << threebythreeident;
493  R = (1 - 2*DotProduct(v_, v_))*R + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
494 
495  R.Release();
496  return R;
497 }
498 
505 {
506  Matrix T(4,4);
507  T << fourbyfourident;
508  T.SubMatrix(1,3,1,3) = (1 - 2*DotProduct(v_, v_))*T.SubMatrix(1,3,1,3)
509  + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
510  T.Release();
511  return T;
512 }
513 
514 // -------------------------------------------------------------------------------------
515 
516 Quaternion operator*(const Real c, const Quaternion & q)
526 {
527  Quaternion out;
528  out.set_s(q.s() * c);
529  out.set_v(q.v() * c);
530  return out;
531 }
532 
533 Quaternion operator*(const Quaternion & q, const Real c)
537 {
538  return operator*(c, q);
539 }
540 
541 
542 Quaternion operator/(const Real c, const Quaternion & q)
548 {
549  Quaternion out;
550  out.set_s(q.s() / c);
551  out.set_v(q.v() / c);
552  return out;
553 }
554 
555 Quaternion operator/(const Quaternion & q, const Real c)
556 {
557  return operator/(c, q);
558 }
559 
560 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot)
566 {
567  Matrix A, B, M;
569  ColumnVector w(3);
570  A = 0.5*q.E(BASE_FRAME);
571  B = q_dot.v();
572  if(A.Determinant())
573  {
574  QRZ(A,U); //QR decomposition
575  QRZ(A,B,M);
576  w = U.i()*M;
577  }
578  else
579  w = 0;
580 
581  w.Release();
582  return w;
583 }
584 
585 short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
586  Quaternion & quat, const Real dt)
588 {
589  if (dt < 0)
590  {
591  cerr << "Integ_Trap(quat1, quat2, dt): dt < 0. dt is set to 0." << endl;
592  return -1;
593  }
594 
595  // Quaternion algebraic constraint
596  // Real Klambda = 0.5*(1 - quat.norm_sqr());
597 
598  dquat_present.set_s(dquat_present.s() );//+ Klambda*quat.s());
599  dquat_present.set_v(dquat_present.v() ); //+ Klambda*quat.v());
600 
601  quat.set_s(quat.s() + Integ_Trap_quat_s(dquat_present, dquat_past, dt));
602  quat.set_v(quat.v() + Integ_Trap_quat_v(dquat_present, dquat_past, dt));
603 
604  dquat_past.set_s(dquat_present.s());
605  dquat_past.set_v(dquat_present.v());
606 
607  quat.unit();
608 
609  return 0;
610 }
611 
612 Real Integ_Trap_quat_s(const Quaternion & present, Quaternion & past,
613  const Real dt)
615 {
616  Real integ = 0.5*(present.s()+past.s())*dt;
617  past.set_s(present.s());
618  return integ;
619 }
620 
622  const Real dt)
624 {
625  ColumnVector integ = 0.5*(present.v()+past.v())*dt;
626  past.set_v(present.v());
627  integ.Release();
628  return integ;
629 }
630 
631 Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const Real t)
682 {
683  if( (t < 0) || (t > 1) )
684  cerr << "Slerp(q0, q1, t): t < 0 or t > 1. t is set to 0." << endl;
685 
686  if(q0.dot_prod(q1) >= 0)
687  return q0*((q0.i()*q1).power(t));
688  else
689  return q0*((q0.i()*-1*q1).power(t));
690 }
691 
693  const Real t)
715 {
716  if( (t < 0) || (t > 1) )
717  cerr << "Slerp_prime(q0, q1, t): t < 0 or t > 1. t is set to 0." << endl;
718 
719  if(q0.dot_prod(q1) >= 0)
720  return Slerp(q0, q1, t)*(q0.i()*q1).Log();
721  else
722  return Slerp(q0, q1, t)*(q0.i()*-1*q1).Log();
723 }
724 
725 Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b,
726  const Quaternion & q, const Real t)
744 {
745  if( (t < 0) || (t > 1) )
746  cerr << "Squad(p,a,b,q, t): t < 0 or t > 1. t is set to 0." << endl;
747 
748  return Slerp(Slerp(p,q,t),Slerp(a,b,t),2*t*(1-t));
749 }
750 
751 Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b,
752  const Quaternion & q, const Real t)
793 {
794  if( (t < 0) || (t > 1) )
795  cerr << "Squad_prime(p,a,b,q, t): t < 0 or t > 1. t is set to 0." << endl;
796 
797  Quaternion q_squad,
798  U = Slerp(p, q, t),
799  V = Slerp(a, b, t),
800  W = U.i()*V,
801  U_prime = U*(p.i()*q).Log(),
802  V_prime = V*(a.i()*b).Log(),
803  W_prime = U.i()*V_prime - U.power(-2)*U_prime*V;
804 
805  q_squad = U*( W.power(2*t*(1-t))*W.Log()*(2-4*t) + W.power(2*t*(1-t)-1)*W_prime*2*t*(1-t) )
806  + U_prime*( W.power(2*t*(1-t)) );
807 
808  return q_squad;
809 }
810 
811 #ifdef use_namespace
812 }
813 #endif
814 
815 
816 
817 
818 
819 
Quaternion class definition.
Definition: quaternion.h:92
ReturnMatrix R() const
Rotation matrix from a unit quaternion.
Definition: quaternion.cpp:458
ReturnMatrix v() const
Return vector part.
Definition: quaternion.h:121
void Release()
Definition: newmat.h:514
Quaternion exp() const
Exponential of a quaternion.
Definition: quaternion.cpp:336
Quaternion()
Constructor.
Definition: quaternion.cpp:77
Real DotProduct(const Matrix &A, const Matrix &B)
Definition: newmat.h:2060
ReturnMatrix x_prod_matrix(const ColumnVector &x)
Cross product matrix.
Definition: utils.cpp:88
double Real
Definition: include.h:307
Quaternion operator/(const Real c, const Quaternion &q)
Overload / operator, division by a scalar.
Definition: quaternion.cpp:542
Quaternion Slerp(const Quaternion &q0, const Quaternion &q1, const Real t)
Spherical Linear Interpolation.
Definition: quaternion.cpp:631
static const char rcsid[]
RCS/CVS version.
Definition: quaternion.cpp:67
int Nrows() const
Definition: newmat.h:494
Real s_
Quaternion scalar part.
Definition: quaternion.h:127
short Integ_quat(Quaternion &dquat_present, Quaternion &dquat_past, Quaternion &quat, const Real dt)
Trapezoidal quaternion integration.
Definition: quaternion.cpp:585
Real s() const
Return scalar part.
Definition: quaternion.h:119
ReturnMatrix Integ_Trap_quat_v(const Quaternion &present, Quaternion &past, const Real dt)
Trapezoidal quaternion vector part integration.
Definition: quaternion.cpp:621
Real Integ_Trap_quat_s(const Quaternion &present, Quaternion &past, const Real dt)
Trapezoidal quaternion scalar part integration.
Definition: quaternion.cpp:612
Upper triangular matrix.
Definition: newmat.h:799
Quaternion power(const Real t) const
Definition: quaternion.cpp:358
void set_s(const Real s)
Set scalar part.
Definition: quaternion.h:120
Quaternion & unit()
Normalize a quaternion.
Definition: quaternion.cpp:311
void set_v(const ColumnVector &v)
Set vector part.
Definition: quaternion.cpp:274
Quaternion conjugate() const
Conjugate.
Definition: quaternion.cpp:283
TransposedMatrix t() const
Definition: newmat6.cpp:320
Quaternion Squad_prime(const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
Spherical Cubic Interpolation derivative.
Definition: quaternion.cpp:751
Quaternion operator+(const Quaternion &q) const
Overload + operator.
Definition: quaternion.cpp:203
Quaternion Slerp_prime(const Quaternion &q0, const Quaternion &q1, const Real t)
Spherical Linear Interpolation derivative.
Definition: quaternion.cpp:692
ReturnMatrix E(const short sign) const
Matrix E.
Definition: quaternion.cpp:426
#define BASE_FRAME
Definition: quaternion.h:84
ColumnVector q0
Definition: demo.cpp:229
Matrix CrossProduct(const Matrix &A, const Matrix &B)
Definition: newmat.h:2062
#define EPSILON
Definition: quaternion.h:86
Quaternion Log() const
Logarithm of a unit quaternion.
Definition: quaternion.cpp:365
The usual rectangular matrix.
Definition: newmat.h:625
Quaternion dot(const ColumnVector &w, const short sign) const
Quaternion time derivative.
Definition: quaternion.cpp:388
InvertedMatrix i() const
Definition: newmat6.cpp:329
Real Determinant() const
Definition: newmat.h:344
FloatVector FloatVector * a
#define BODY_FRAME
Definition: quaternion.h:85
Real dot_prod(const Quaternion &q) const
Quaternion dot product.
Definition: quaternion.cpp:445
FloatVector * angle
Quaternion Squad(const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
Spherical Cubic Interpolation.
Definition: quaternion.cpp:725
int Ncols() const
Definition: newmat.h:495
void QRZ(Matrix &X, UpperTriangularMatrix &U)
Definition: hholder.cpp:117
ColumnVector v_
Quaternion vector part.
Definition: quaternion.h:128
Real norm() const
Return the quaternion norm.
Definition: quaternion.cpp:298
Quaternion class.
Quaternion i() const
Quaternion inverse. where and are the quaternion conjugate and the quaternion norm respectively...
Definition: quaternion.cpp:323
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
Definition: newmat.h:2146
Quaternion operator*(const Real c, const Quaternion &q)
Overload * operator, multiplication by a scalar.
Definition: quaternion.cpp:516
Quaternion operator/(const Quaternion &q) const
Overload / operator.
Definition: quaternion.cpp:267
Real threebythreeident[]
Used to initialize a matrix.
Definition: robot.cpp:136
Column vector.
Definition: newmat.h:1008
Real fourbyfourident[]
Used to initialize a matrix.
Definition: robot.cpp:133
Quaternion operator*(const Quaternion &q) const
Overload * operator.
Definition: quaternion.cpp:243
ReturnMatrix T() const
Transformation matrix from a quaternion.
Definition: quaternion.cpp:499
ReturnMatrix Omega(const Quaternion &q, const Quaternion &q_dot)
Return angular velocity from a quaternion and it&#39;s time derivative.
Definition: quaternion.cpp:560
Quaternion operator-(const Quaternion &q) const
Overload - operator.
Definition: quaternion.cpp:223


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16