Developer Documentation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
DualQuaternionT.cc
1 /*===========================================================================*\
2  * *
3  * OpenFlipper *
4  * Copyright (c) 2001-2015, RWTH-Aachen University *
5  * Department of Computer Graphics and Multimedia *
6  * All rights reserved. *
7  * www.openflipper.org *
8  * *
9  *---------------------------------------------------------------------------*
10  * This file is part of OpenFlipper. *
11  *---------------------------------------------------------------------------*
12  * *
13  * Redistribution and use in source and binary forms, with or without *
14  * modification, are permitted provided that the following conditions *
15  * are met: *
16  * *
17  * 1. Redistributions of source code must retain the above copyright notice, *
18  * this list of conditions and the following disclaimer. *
19  * *
20  * 2. Redistributions in binary form must reproduce the above copyright *
21  * notice, this list of conditions and the following disclaimer in the *
22  * documentation and/or other materials provided with the distribution. *
23  * *
24  * 3. Neither the name of the copyright holder nor the names of its *
25  * contributors may be used to endorse or promote products derived from *
26  * this software without specific prior written permission. *
27  * *
28  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS *
29  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
30  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A *
31  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
32  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, *
33  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, *
34  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR *
35  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF *
36  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING *
37  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
38  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
39  * *
40 \*===========================================================================*/
41 
42 /*===========================================================================*\
43  * *
44  * $Revision: 9595 $ *
45  * $Author: wilden $ *
46  * $Date: 2010-06-17 12:48:23 +0200 (Thu, 17 Jun 2010) $ *
47  * *
48 \*===========================================================================*/
49 
50 
51 
52 //=============================================================================
53 //
54 // CLASS DualQuaternionT - IMPLEMENTATION
55 //
56 //=============================================================================
57 
58 
59 #define ACG_DUALQUATERNIONT_C
60 
61 
62 //== INCLUDES =================================================================
63 
64 
65 #include "DualQuaternionT.hh"
66 #include <iostream>
67 
68 //== IMPLEMENTATION ==========================================================
69 
70 
71 namespace ACG {
72 
73  //-----------------------------------------------------------------------------
74 
76  template <typename Scalar>
78  *this = DualQuaternion::identity();
79  }
80 
81  //-----------------------------------------------------------------------------
82 
84  template <typename Scalar>
86  real_ = _other.real_;
87  dual_ = _other.dual_;
88  }
89 
90  //-----------------------------------------------------------------------------
91 
93  template <typename Scalar>
95  real_ = _real;
96  dual_ = _dual;
97  }
98 
99  //-----------------------------------------------------------------------------
100 
102  template <typename Scalar>
103  DualQuaternionT<Scalar>::DualQuaternionT(Scalar _Rw, Scalar _Rx, Scalar _Ry, Scalar _Rz,
104  Scalar _Dw, Scalar _Dx, Scalar _Dy, Scalar _Dz){
105  real_ = Quaternion(_Rw, _Rx, _Ry, _Rz);
106  dual_ = Quaternion(_Dw, _Dx, _Dy, _Dz);
107  }
108 
109  //-----------------------------------------------------------------------------
110 
112  template <typename Scalar>
114  real_ = _rotation;
115  dual_ = Quaternion(0.0,0.0,0.0,0.0);
116  }
117 
118  //-----------------------------------------------------------------------------
119 
121  template <typename Scalar>
123  real_.identity();
124  dual_ = Quaternion( 0.0, 0.5 * _translation[0], 0.5 * _translation[1], 0.5 * _translation[2] );
125  }
126 
127  //-----------------------------------------------------------------------------
128 
130  template <typename Scalar>
131  DualQuaternionT<Scalar>::DualQuaternionT(const Vec3& _translation, const Quaternion& _rotation){
132 
133  real_ = _rotation;
134  dual_ = Quaternion( 0.0, 0.5 * _translation[0], 0.5 * _translation[1], 0.5 * _translation[2] );
135 
136  dual_ *= real_;
137  }
138 
139  //-----------------------------------------------------------------------------
140 
142  template <typename Scalar>
144  real_ = Quaternion(_transformation); //the quaternion constructor ignores the translation
145  dual_ = Quaternion( 0.0, 0.5 * _transformation(0,3), 0.5 * _transformation(1,3), 0.5 * _transformation(2,3) );
146 
147  dual_ *= real_;
148  }
149 
150  //-----------------------------------------------------------------------------
151 
153  template <typename Scalar>
155 
156  Quaternion real;
157  real.identity();
158 
159  Quaternion dual = Quaternion(0.0, 0.0, 0.0, 0.0);
160 
161  return DualQuaternion( real, dual );
162  }
163 
164  //-----------------------------------------------------------------------------
165 
167  template <typename Scalar>
169 
170  Quaternion real = Quaternion(0.0, 0.0, 0.0, 0.0);
171  Quaternion dual = Quaternion(0.0, 0.0, 0.0, 0.0);
172 
173  return DualQuaternion( real, dual );
174  }
175 
176  //-----------------------------------------------------------------------------
177 
179  template <typename Scalar>
181  return DualQuaternion( real_.conjugate(), dual_.conjugate() );
182  }
183 
184  //-----------------------------------------------------------------------------
185 
187  template <typename Scalar>
189 
190  double sqrLen0 = real_.sqrnorm();
191  double sqrLenE = 2.0 * (real_ | dual_);
192 
193  if ( sqrLen0 > 0.0 ){
194 
195  double invSqrLen0 = 1.0/sqrLen0;
196  double invSqrLenE = -sqrLenE/(sqrLen0*sqrLen0);
197 
198  DualQuaternion conj = conjugate();
199 
200  conj.real_ = invSqrLen0 * conj.real_;
201  conj.dual_ = invSqrLen0 * conj.dual_ + invSqrLenE * conj.real_;
202 
203  return conj;
204 
205  } else
206  return DualQuaternion::zero();
207  }
208 
209  //-----------------------------------------------------------------------------
210 
212  template <typename Scalar>
214 
215  const double magn = 1.0/real_.norm();
216  const double magnSqr = 1.0/real_.sqrnorm();
217 
218  // normalize rotation
219  real_ *= magn;
220  dual_ *= magn;
221 
222  // normalize the rest
223  dual_ -= ((real_| dual_)* magnSqr) * real_;
224 
225  }
226 
227  //-----------------------------------------------------------------------------
228 
230  template <typename Scalar>
232  return (_other.real_ == real_) && (_other.dual_ == dual_);
233  }
234 
235  //-----------------------------------------------------------------------------
236 
238  template <typename Scalar>
240  return (_other.real_ != real_) || (_other.dual_ != dual_);
241  }
242 
243  //-----------------------------------------------------------------------------
244 
246  template <typename Scalar>
248  return DualQuaternion( real_ + _other.real_, dual_ + _other.dual_ );
249  }
250 
251  //-----------------------------------------------------------------------------
252 
254  template <typename Scalar>
256  real_ = real_ + _other.real_;
257  dual_ = dual_ + _other.dual_;
258 
259  return (*this);
260  }
261 
262  //-----------------------------------------------------------------------------
263 
265  template <typename Scalar>
267  return DualQuaternion( real_ - _other.real_, dual_ - _other.dual_ );
268  }
269 
270  //-----------------------------------------------------------------------------
271 
273  template <typename Scalar>
275  real_ -= _other.real_;
276  dual_ -= _other.dual_;
277 
278  return (*this);
279  }
280 
281  //-----------------------------------------------------------------------------
282 
284  template <typename Scalar>
286  return DualQuaternion( real_ * _q.real_, real_ * _q.dual_ + dual_ * _q.real_ );
287  }
288 
289  //-----------------------------------------------------------------------------
290 
292  template <typename Scalar>
294  dual_ = real_ * _q.dual_ + dual_ * _q.real_;
295  real_ *= _q.real_;
296 
297  return (*this);
298  }
299 
300  //-----------------------------------------------------------------------------
301 
303  template <typename Scalar>
305  DualQuaternion q;
306 
307  q.real_ = real_ * _scalar;
308  q.dual_ = dual_ * _scalar;
309 
310  return q;
311  }
312 
313  //-----------------------------------------------------------------------------
314 
315  template <typename Scalar>
316  Scalar& DualQuaternionT<Scalar>::operator [](const unsigned int& b) {
317  if ( b < 4 ) {
318  return real_[b];
319  } else if ( b < 8 ) {
320  return dual_[b - 4];
321  } else {
322  // Invalid operation, write error and return anything.
323  std::cerr << "Error in Dualquaternion operator[], index b out of range [0...7]" << std::endl;
324  return real_[0];
325  }
326  }
327 
328 
329  //-----------------------------------------------------------------------------
330 
332  template <typename Scalar> template<typename VectorType>
333  DualQuaternionT<Scalar> DualQuaternionT<Scalar>::interpolate(VectorType& _weights, const std::vector<DualQuaternion>& _dualQuaternions)
334  {
335  if ( (_weights.size() != _dualQuaternions.size()) || (_weights.size() == 0) ){
336  std::cerr << "Cannot interpolate dual quaternions ( weights: " << _weights.size() << ", DQs: " << _dualQuaternions.size() << std::endl;
337  return DualQuaternionT::zero();
338  }
339 
340  // Find max weight for pivoting to that quaternion,
341  // so shortest arc is taken (see: 'coping antipodality' in the paper )
342  uint pivotID = 0;
343 
344  for (uint i=1; i<_dualQuaternions.size(); i++)
345  if (_weights[pivotID] < _weights[i])
346  pivotID = i;
347 
348  DualQuaternion pivotDQ = _dualQuaternions[ pivotID ];
349  Quaternion pivotQ = pivotDQ.real_;
350 
351  DualQuaternion res = DualQuaternion::zero();
352 
353  for (uint i=0; i<_dualQuaternions.size(); i++){
354 
355  DualQuaternion currentDQ = _dualQuaternions[i];
356  Quaternion currentQ = currentDQ.real_;
357 
358  // Make sure dot product is >= 0
359  if ( ( currentQ | pivotQ ) < 0.0 )
360  _weights[i] = -_weights[i];
361 
362  res += _dualQuaternions[i] * _weights[i];
363  }
364 
365  res.normalize();
366 
367  return res;
368  }
369 
370  //-----------------------------------------------------------------------------
371 
373  template <typename Scalar>
375  {
377 
378  // for details about the calculation see the paper (algorithm 1)
379 
380  Vec3 p(_point);
381 
382  double r = real_[0];
383  Vec3 rv = Vec3(real_[1], real_[2], real_[3]);
384 
385  double d = dual_[0];
386  Vec3 dv = Vec3(dual_[1], dual_[2], dual_[3]);
387 
388  Vec3 tempVec = (rv % p) + r * p;
389  p+=2.0*(rv % tempVec);
390 
391  Vec3 t = dv % rv;
392  t+= d * rv;
393  t+= -r*dv;
394  p+=-2.0*t;
395 
396  return p;
397  }
398 
399  //-----------------------------------------------------------------------------
400 
402  template <typename Scalar>
404  {
406 
407  // for details about the calculation see the paper (algorithm 1)
408 
409  Vec3 p(_point);
410 
411  double r = real_[0];
412  Vec3 rv = Vec3(real_[1], real_[2], real_[3]);
413 
414  Vec3 tempVec = (rv % p) + r * p;
415  p+=2.0*(rv % tempVec);
416 
417  return p;
418 }
419 
420  //-----------------------------------------------------------------------------
421 
423  template <typename Scalar>
425 
426  std::cerr << "Real Part:" << std::endl;
427  real_.print_info();
428  std::cerr << "Dual Part:" << std::endl;
429  dual_.print_info();
430  }
431 
432  //-----------------------------------------------------------------------------
433 
434 //=============================================================================
435 } // namespace ACG
436 //=============================================================================
bool operator!=(const DualQuaternion &_other) const
dual quaternion comparison
DualQuaternionT()
Default constructor ( constructs an identity dual quaternion )
Namespace providing different geometric functions concerning angles.
Definition: DBSCANT.cc:51
void identity()
identity rotation
Definition: QuaternionT.hh:129
DualQuaternion & operator*=(const DualQuaternion &_q)
dualQuaternion *= dualQuaternion
DualQuaternion conjugate() const
conjugate dual quaternion
void printInfo()
print some info about the DQ
Quaternion real_
real and dual quaternion parts
void normalize()
normalize dual quaternion
DualQuaternion & operator-=(const DualQuaternion &_other)
substraction
Vec3 transform_point(const Vec3 &_point) const
Transform a point with the dual quaternion.
static DualQuaternion interpolate(VectorType &_weights, const std::vector< DualQuaternion > &_dualQuaternions)
linear interpolation of dual quaternions. Result is normalized afterwards
DualQuaternion class for representing rigid motions in 3d.
DualQuaternion operator-(const DualQuaternion &_other) const
substraction
static DualQuaternion zero()
zero dual quaternion [ R(0, 0, 0, 0), D(0,0,0,0) ]
Vec3 transform_vector(const Vec3 &_point) const
Transform a vector with the dual quaternion.
Scalar & operator[](const unsigned int &b)
Access as one big vector.
static DualQuaternion identity()
identity dual quaternion [ R(1, 0, 0, 0), D(0,0,0,0) ]
DualQuaternion & operator+=(const DualQuaternion &_other)
addition
bool operator==(const DualQuaternion &_other) const
dual quaternion comparison
DualQuaternion operator*(const DualQuaternion &_q) const
dualQuaternion * dualQuaternion
DualQuaternion operator+(const DualQuaternion &_other) const
addition
DualQuaternion invert() const
invert dual quaternion