Developer Documentation

DualQuaternionT_impl.hh
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 //=============================================================================
45 //
46 // CLASS DualQuaternionT - IMPLEMENTATION
47 //
48 //=============================================================================
49
50
51 #define ACG_DUALQUATERNIONT_C
52
53
54 //== INCLUDES =================================================================
55
56
57 #include "DualQuaternionT.hh"
58 #include <iostream>
59
60 //== IMPLEMENTATION ==========================================================
61
62
63 namespace ACG {
64
65 //-----------------------------------------------------------------------------
66
68 template <typename Scalar>
69 DualQuaternionT<Scalar>::DualQuaternionT(){
70 *this = DualQuaternion::identity();
71 }
72
73 //-----------------------------------------------------------------------------
74
76 template <typename Scalar>
77 DualQuaternionT<Scalar>::DualQuaternionT(const DualQuaternion& _other){
78 real_ = _other.real_;
79 dual_ = _other.dual_;
80 }
81
82 //-----------------------------------------------------------------------------
83
85 template <typename Scalar>
86 DualQuaternionT<Scalar>::DualQuaternionT(const Quaternion& _real, const Quaternion& _dual){
87 real_ = _real;
88 dual_ = _dual;
89 }
90
91 //-----------------------------------------------------------------------------
92
94 template <typename Scalar>
95 DualQuaternionT<Scalar>::DualQuaternionT(Scalar _Rw, Scalar _Rx, Scalar _Ry, Scalar _Rz,
96 Scalar _Dw, Scalar _Dx, Scalar _Dy, Scalar _Dz){
97 real_ = Quaternion(_Rw, _Rx, _Ry, _Rz);
98 dual_ = Quaternion(_Dw, _Dx, _Dy, _Dz);
99 }
100
101 //-----------------------------------------------------------------------------
102
104 template <typename Scalar>
105 DualQuaternionT<Scalar>::DualQuaternionT(Quaternion _rotation){
106 real_ = _rotation;
107 dual_ = Quaternion(0.0,0.0,0.0,0.0);
108 }
109
110 //-----------------------------------------------------------------------------
111
113 template <typename Scalar>
114 DualQuaternionT<Scalar>::DualQuaternionT(const Vec3& _translation){
115 real_.identity();
116 dual_ = Quaternion( 0.0, 0.5 * _translation[0], 0.5 * _translation[1], 0.5 * _translation[2] );
117 }
118
119 //-----------------------------------------------------------------------------
120
122 template <typename Scalar>
123 DualQuaternionT<Scalar>::DualQuaternionT(const Vec3& _translation, const Quaternion& _rotation){
124
125 real_ = _rotation;
126 dual_ = Quaternion( 0.0, 0.5 * _translation[0], 0.5 * _translation[1], 0.5 * _translation[2] );
127
128 dual_ *= real_;
129 }
130
131 //-----------------------------------------------------------------------------
132
134 template <typename Scalar>
135 DualQuaternionT<Scalar>::DualQuaternionT(const Matrix& _transformation){
136 real_ = Quaternion(_transformation); //the quaternion constructor ignores the translation
137 dual_ = Quaternion( 0.0, 0.5 * _transformation(0,3), 0.5 * _transformation(1,3), 0.5 * _transformation(2,3) );
138
139 dual_ *= real_;
140 }
141
142 //-----------------------------------------------------------------------------
143
145 template <typename Scalar>
146 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::identity(){
147
148 Quaternion real;
149 real.identity();
150
151 Quaternion dual = Quaternion(0.0, 0.0, 0.0, 0.0);
152
153 return DualQuaternion( real, dual );
154 }
155
156 //-----------------------------------------------------------------------------
157
159 template <typename Scalar>
160 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::zero(){
161
162 Quaternion real = Quaternion(0.0, 0.0, 0.0, 0.0);
163 Quaternion dual = Quaternion(0.0, 0.0, 0.0, 0.0);
164
165 return DualQuaternion( real, dual );
166 }
167
168 //-----------------------------------------------------------------------------
169
171 template <typename Scalar>
172 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::conjugate() const {
173 return DualQuaternion( real_.conjugate(), dual_.conjugate() );
174 }
175
176 //-----------------------------------------------------------------------------
177
179 template <typename Scalar>
180 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::invert() const {
181
182 double sqrLen0 = real_.sqrnorm();
183 double sqrLenE = 2.0 * (real_ | dual_);
184
185 if ( sqrLen0 > 0.0 ){
186
187 double invSqrLen0 = 1.0/sqrLen0;
188 double invSqrLenE = -sqrLenE/(sqrLen0*sqrLen0);
189
190 DualQuaternion conj = conjugate();
191
192 conj.real_ = invSqrLen0 * conj.real_;
193 conj.dual_ = invSqrLen0 * conj.dual_ + invSqrLenE * conj.real_;
194
195 return conj;
196
197 } else
198 return DualQuaternion::zero();
199 }
200
201 //-----------------------------------------------------------------------------
202
204 template <typename Scalar>
205 void DualQuaternionT<Scalar>::normalize() {
206
207 const double magn = 1.0/real_.norm();
208 const double magnSqr = 1.0/real_.sqrnorm();
209
210 // normalize rotation
211 real_ *= magn;
212 dual_ *= magn;
213
214 // normalize the rest
215 dual_ -= ((real_| dual_)* magnSqr) * real_;
216
217 }
218
219 //-----------------------------------------------------------------------------
220
222 template <typename Scalar>
223 bool DualQuaternionT<Scalar>::operator==(const DualQuaternion& _other) const {
224 return (_other.real_ == real_) && (_other.dual_ == dual_);
225 }
226
227 //-----------------------------------------------------------------------------
228
230 template <typename Scalar>
231 bool DualQuaternionT<Scalar>::operator!=(const DualQuaternion& _other) const {
232 return (_other.real_ != real_) || (_other.dual_ != dual_);
233 }
234
235 //-----------------------------------------------------------------------------
236
238 template <typename Scalar>
239 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::operator+(const DualQuaternion& _other) const {
240 return DualQuaternion( real_ + _other.real_, dual_ + _other.dual_ );
241 }
242
243 //-----------------------------------------------------------------------------
244
246 template <typename Scalar>
247 DualQuaternionT<Scalar>& DualQuaternionT<Scalar>::operator+=(const DualQuaternion& _other) {
248 real_ = real_ + _other.real_;
249 dual_ = dual_ + _other.dual_;
250
251 return (*this);
252 }
253
254 //-----------------------------------------------------------------------------
255
257 template <typename Scalar>
258 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::operator-(const DualQuaternion& _other) const {
259 return DualQuaternion( real_ - _other.real_, dual_ - _other.dual_ );
260 }
261
262 //-----------------------------------------------------------------------------
263
265 template <typename Scalar>
266 DualQuaternionT<Scalar>& DualQuaternionT<Scalar>::operator-=(const DualQuaternion& _other) {
267 real_ -= _other.real_;
268 dual_ -= _other.dual_;
269
270 return (*this);
271 }
272
273 //-----------------------------------------------------------------------------
274
276 template <typename Scalar>
277 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::operator*(const DualQuaternion& _q) const {
278 return DualQuaternion( real_ * _q.real_, real_ * _q.dual_ + dual_ * _q.real_ );
279 }
280
281 //-----------------------------------------------------------------------------
282
284 template <typename Scalar>
285 DualQuaternionT<Scalar>& DualQuaternionT<Scalar>::operator*=(const DualQuaternion& _q){
286 dual_ = real_ * _q.dual_ + dual_ * _q.real_;
287 real_ *= _q.real_;
288
289 return (*this);
290 }
291
292 //-----------------------------------------------------------------------------
293
295 template <typename Scalar>
296 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::operator*(const Scalar& _scalar) const {
297 DualQuaternion q;
298
299 q.real_ = real_ * _scalar;
300 q.dual_ = dual_ * _scalar;
301
302 return q;
303 }
304
305 //-----------------------------------------------------------------------------
306
307 template <typename Scalar>
308 Scalar& DualQuaternionT<Scalar>::operator [](const unsigned int& b) {
309 if ( b < 4 ) {
310 return real_[b];
311 } else if ( b < 8 ) {
312 return dual_[b - 4];
313 } else {
314 // Invalid operation, write error and return anything.
315 std::cerr << "Error in Dualquaternion operator[], index b out of range [0...7]" << std::endl;
316 return real_[0];
317 }
318 }
319
320
321 //-----------------------------------------------------------------------------
322
324 template <typename Scalar> template<typename VectorType>
325 DualQuaternionT<Scalar> DualQuaternionT<Scalar>::interpolate(VectorType& _weights, const std::vector<DualQuaternion>& _dualQuaternions)
326 {
327 if ( (_weights.size() != _dualQuaternions.size()) || (_weights.size() == 0) ){
328 std::cerr << "Cannot interpolate dual quaternions ( weights: " << _weights.size() << ", DQs: " << _dualQuaternions.size() << std::endl;
329 return DualQuaternionT::zero();
330 }
331
332 // Find max weight for pivoting to that quaternion,
333 // so shortest arc is taken (see: 'coping antipodality' in the paper )
334 uint pivotID = 0;
335
336 for (uint i=1; i<_dualQuaternions.size(); i++)
337 if (_weights[pivotID] < _weights[i])
338 pivotID = i;
339
340 DualQuaternion pivotDQ = _dualQuaternions[ pivotID ];
341 Quaternion pivotQ = pivotDQ.real_;
342
343 DualQuaternion res = DualQuaternion::zero();
344
345 for (uint i=0; i<_dualQuaternions.size(); i++){
346
347 DualQuaternion currentDQ = _dualQuaternions[i];
348 Quaternion currentQ = currentDQ.real_;
349
350 // Make sure dot product is >= 0
351 if ( ( currentQ | pivotQ ) < 0.0 )
352 _weights[i] = -_weights[i];
353
354 res += _dualQuaternions[i] * _weights[i];
355 }
356
357 res.normalize();
358
359 return res;
360 }
361
362 //-----------------------------------------------------------------------------
363
365 template <typename Scalar>
366 VectorT<Scalar,3> DualQuaternionT<Scalar>::transform_point(const Vec3& _point) const
367 {
369
370 // for details about the calculation see the paper (algorithm 1)
371
372 Vec3 p(_point);
373
374 double r = real_[0];
375 Vec3 rv = Vec3(real_[1], real_[2], real_[3]);
376
377 double d = dual_[0];
378 Vec3 dv = Vec3(dual_[1], dual_[2], dual_[3]);
379
380 Vec3 tempVec = (rv % p) + r * p;
381 p+=2.0*(rv % tempVec);
382
383 Vec3 t = dv % rv;
384 t+= d * rv;
385 t+= -r*dv;
386 p+=-2.0*t;
387
388 return p;
389 }
390
391 //-----------------------------------------------------------------------------
392
394 template <typename Scalar>
395 VectorT<Scalar,3> DualQuaternionT<Scalar>::transform_vector(const Vec3& _point) const
396 {
398
399 // for details about the calculation see the paper (algorithm 1)
400
401 Vec3 p(_point);
402
403 double r = real_[0];
404 Vec3 rv = Vec3(real_[1], real_[2], real_[3]);
405
406 Vec3 tempVec = (rv % p) + r * p;
407 p+=2.0*(rv % tempVec);
408
409 return p;
410 }
411
412 //-----------------------------------------------------------------------------
413
415 template <typename Scalar>
416 void DualQuaternionT<Scalar>::printInfo(){
417
418 std::cerr << "Real Part:" << std::endl;
419 real_.print_info();
420 std::cerr << "Dual Part:" << std::endl;
421 dual_.print_info();
422 }
423
424 //-----------------------------------------------------------------------------
425
426 //=============================================================================
427 } // namespace ACG
428 //=============================================================================
ACG
Namespace providing different geometric functions concerning angles.
Definition: BaseObjectData.hh:68
ACG::DualQuaternionT::operator!=
bool operator!=(const DualQuaternion &_other) const
dual quaternion comparison
Definition: DualQuaternionT_impl.hh:231
ACG::DualQuaternionT::real_
Quaternion real_
real and dual quaternion parts
Definition: DualQuaternionT.hh:91
ACG::DualQuaternionT::printInfo
void printInfo()
print some info about the DQ
Definition: DualQuaternionT_impl.hh:416
ACG::DualQuaternionT::normalize
void normalize()
normalize dual quaternion
Definition: DualQuaternionT_impl.hh:205
ACG::DualQuaternionT::operator-=
DualQuaternion & operator-=(const DualQuaternion &_other)
substraction
Definition: DualQuaternionT_impl.hh:266
ACG::DualQuaternionT::operator-
DualQuaternion operator-(const DualQuaternion &_other) const
substraction
Definition: DualQuaternionT_impl.hh:258
ACG::DualQuaternionT::conjugate
DualQuaternion conjugate() const
conjugate dual quaternion
Definition: DualQuaternionT_impl.hh:172
ACG::DualQuaternionT::invert
DualQuaternion invert() const
invert dual quaternion
Definition: DualQuaternionT_impl.hh:180
ACG::QuaternionT
Definition: QuaternionT.hh:75
ACG::DualQuaternionT::operator*
DualQuaternion operator*(const DualQuaternion &_q) const
dualQuaternion * dualQuaternion
Definition: DualQuaternionT_impl.hh:277
ACG::DualQuaternionT::transform_vector
Vec3 transform_vector(const Vec3 &_point) const
Transform a vector with the dual quaternion.
Definition: DualQuaternionT_impl.hh:395
ACG::DualQuaternionT::operator+=
DualQuaternion & operator+=(const DualQuaternion &_other)
addition
Definition: DualQuaternionT_impl.hh:247
ACG::DualQuaternionT::operator*=
DualQuaternion & operator*=(const DualQuaternion &_q)
dualQuaternion *= dualQuaternion
Definition: DualQuaternionT_impl.hh:285
ACG::DualQuaternionT::interpolate
static DualQuaternion interpolate(VectorType &_weights, const std::vector< DualQuaternion > &_dualQuaternions)
linear interpolation of dual quaternions. Result is normalized afterwards
ACG::Matrix4x4T
Definition: Matrix4x4T.hh:92
ACG::DualQuaternionT::operator+
DualQuaternion operator+(const DualQuaternion &_other) const
addition
Definition: DualQuaternionT_impl.hh:239
ACG::QuaternionT::identity
void identity()
identity rotation
Definition: QuaternionT.hh:123
ACG::DualQuaternionT
DualQuaternion class for representing rigid motions in 3d.
Definition: DualQuaternionT.hh:80
ACG::DualQuaternionT::DualQuaternionT
DualQuaternionT()
Default constructor ( constructs an identity dual quaternion )
Definition: DualQuaternionT_impl.hh:69
ACG::DualQuaternionT::operator==
bool operator==(const DualQuaternion &_other) const
dual quaternion comparison
Definition: DualQuaternionT_impl.hh:223
OpenMesh::VectorT
Definition: Vector11T.hh:83
ACG::DualQuaternionT::zero
static DualQuaternion zero()
zero dual quaternion [ R(0, 0, 0, 0), D(0,0,0,0) ]
Definition: DualQuaternionT_impl.hh:160
ACG::DualQuaternionT::identity
static DualQuaternion identity()
identity dual quaternion [ R(1, 0, 0, 0), D(0,0,0,0) ]
Definition: DualQuaternionT_impl.hh:146
ACG::DualQuaternionT::transform_point
Vec3 transform_point(const Vec3 &_point) const
Transform a point with the dual quaternion.
Definition: DualQuaternionT_impl.hh:366
ACG::DualQuaternionT::operator[]
Scalar & operator[](const unsigned int &b)
Access as one big vector.
Definition: DualQuaternionT_impl.hh:308