Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
DualQuaternionInl.hh
1 /*
2 This file is part of the BIAS library (Basic ImageAlgorithmS).
3 
4 Copyright (C) 2003-2009 (see file CONTACT for details)
5  Multimediale Systeme der Informationsverarbeitung
6  Institut fuer Informatik
7  Christian-Albrechts-Universitaet Kiel
8 
9 
10 BIAS is free software; you can redistribute it and/or modify
11 it under the terms of the GNU Lesser General Public License as published by
12 the Free Software Foundation; either version 2.1 of the License, or
13 (at your option) any later version.
14 
15 BIAS is distributed in the hope that it will be useful,
16 but WITHOUT ANY WARRANTY; without even the implied warranty of
17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 GNU Lesser General Public License for more details.
19 
20 You should have received a copy of the GNU Lesser General Public License
21 along with BIAS; if not, write to the Free Software
22 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 */
24 
25 
26 template <class QUAT_TYPE> inline
27 DualQuaternion<QUAT_TYPE>::DualQuaternion()
28 {
29  //SetIdentity();
30 }
31 
32 template <class QUAT_TYPE> inline
33 DualQuaternion<QUAT_TYPE>::DualQuaternion(const DualQuaternion<QUAT_TYPE> &d)
34 {
35  real = d.real;
36  dual = d.dual;
37 }
38 
39 template <class QUAT_TYPE> inline
40 DualQuaternion<QUAT_TYPE>::DualQuaternion(const Quaternion<QUAT_TYPE> &q,
41  const Quaternion<QUAT_TYPE> &p)
42 {
43  real = q;
44  dual = p;
45 }
46 
47 template <class QUAT_TYPE> inline void
48 DualQuaternion<QUAT_TYPE>::Invert()
49 {
50  real.Invert();
51  dual.Invert();
52 }
53 
54 template <class QUAT_TYPE> inline DualQuaternion<QUAT_TYPE>
55 DualQuaternion<QUAT_TYPE>::Inverse() const
56 {
57  DualQuaternion<QUAT_TYPE> res(*this);
58  res.Invert();
59  return res;
60 }
61 
62 template <class QUAT_TYPE> inline void
63 DualQuaternion<QUAT_TYPE>::MakeUnique()
64 {
65  // @todo consider all cases from Quaternion::MakeUnique()!
66  if (real[3] < 0) {
67  real *= 1.0;
68  dual *= 1.0;
69  }
70 }
71 
72 template <class QUAT_TYPE> inline void
73 DualQuaternion<QUAT_TYPE>::Normalize()
74 {
75  // enforce unit length constraint for real part
76  real.Normalize();
77  // enforce orthogonality of real and dual part
78  dual -= dual.ScalarProduct(real) * real;
79 }
80 
81 template <class QUAT_TYPE> inline void
82 DualQuaternion<QUAT_TYPE>::GetDualNorm(QUAT_TYPE &r, QUAT_TYPE &d) const
83 {
84  // real part of result is norm of real quaternion part
85  r = (QUAT_TYPE)real.NormL2();
86  // dual part of result is inner product of real and dual quaternion parts
87  d = (QUAT_TYPE)(real.ScalarProduct(dual) + dual.ScalarProduct(real));
88 }
89 
90 template <class QUAT_TYPE> inline void
91 DualQuaternion<QUAT_TYPE>::Add(const DualQuaternion<QUAT_TYPE>& r)
92 {
93  real += r.real;
94  dual += r.dual;
95 }
96 
97 template <class QUAT_TYPE> inline void
98 DualQuaternion<QUAT_TYPE>::Add(const DualQuaternion<QUAT_TYPE>& r,
99  DualQuaternion<QUAT_TYPE>& res) const
100 {
101  res = *this;
102  res.Add(r);
103 }
104 
105 template <class QUAT_TYPE> inline void
106 DualQuaternion<QUAT_TYPE>::Sub(const DualQuaternion<QUAT_TYPE>& r)
107 {
108  real -= r.real;
109  dual -= r.dual;
110 }
111 
112 template <class QUAT_TYPE> inline void
113 DualQuaternion<QUAT_TYPE>::Sub(const DualQuaternion<QUAT_TYPE>& r,
114  DualQuaternion<QUAT_TYPE>& res) const
115 {
116  res = *this;
117  res.Sub(r);
118 }
119 
120 template <class QUAT_TYPE> inline void
121 DualQuaternion<QUAT_TYPE>::Mult(const QUAT_TYPE &scalar)
122 {
123  real *= scalar;
124  dual *= scalar;
125 }
126 
127 template <class QUAT_TYPE> inline void
128 DualQuaternion<QUAT_TYPE>::Mult(const QUAT_TYPE &scalar,
129  DualQuaternion<QUAT_TYPE> &res) const
130 {
131  res = *this;
132  res.Mult(scalar);
133 }
134 
135 template <class QUAT_TYPE> inline void
136 DualQuaternion<QUAT_TYPE>::Div(const QUAT_TYPE &scalar)
137 {
138  real /= scalar;
139  dual /= scalar;
140 }
141 
142 template <class QUAT_TYPE> inline void
143 DualQuaternion<QUAT_TYPE>::Div(const QUAT_TYPE &scalar,
144  DualQuaternion<QUAT_TYPE> &res) const
145 {
146  res = *this;
147  res.Div(scalar);
148 }
149 
150 // ---------------------------------------------
151 
152 template <class QUAT_TYPE> inline void
153 DualQuaternion<QUAT_TYPE>::Mult(const DualQuaternion<QUAT_TYPE> &r)
154 {
155  dual = (real * r.dual) + (dual * r.real);
156  real.Mult(r.real);
157 }
158 
159 template <class QUAT_TYPE> inline void
160 DualQuaternion<QUAT_TYPE>::Mult(const DualQuaternion<QUAT_TYPE> &r,
161  DualQuaternion<QUAT_TYPE> &res) const
162 {
163  res = *this;
164  res.Mult(r);
165 }
166 
167 template <class QUAT_TYPE> inline void
168 DualQuaternion<QUAT_TYPE>::MultLeft(const DualQuaternion<QUAT_TYPE> &l)
169 {
170  dual = (l.real * dual) + (l.dual * real);
171  real.MultLeft(l.real);
172 }
173 
174 template <class QUAT_TYPE> inline void
175 DualQuaternion<QUAT_TYPE>::MultLeft(const DualQuaternion<QUAT_TYPE> &l,
176  DualQuaternion<QUAT_TYPE> &res) const
177 {
178  res = *this;
179  res.MultLeft(l);
180 }
181 
182 template <class QUAT_TYPE> inline void
183 DualQuaternion<QUAT_TYPE>::MultVec(const Vector3<QUAT_TYPE> &vec,
184  Vector3<QUAT_TYPE> &res) const
185 {
186  // @todo
187 }
188 
189 template <class QUAT_TYPE> inline Vector3<QUAT_TYPE>
190 DualQuaternion<QUAT_TYPE>::MultVec(const Vector3<QUAT_TYPE> &vec) const
191 {
192  Vector3<QUAT_TYPE> res;
193  MultVec(vec, res);
194  return res;
195 }
196 
197 // ---------------------------------------------
198 
199 template <class QUAT_TYPE> inline void
200 DualQuaternion<QUAT_TYPE>::SetIdentity()
201 {
202  real.SetIdentity();
203  dual.SetZero();
204 }
205 
206 template <class QUAT_TYPE> inline void
207 DualQuaternion<QUAT_TYPE>::Set(const Quaternion<QUAT_TYPE> &q,
208  const Quaternion<QUAT_TYPE> &p)
209 {
210  real = q;
211  dual = p;
212 }
213 
214 template <class QUAT_TYPE> inline void
215 DualQuaternion<QUAT_TYPE>::SetRealPart(const Quaternion<QUAT_TYPE> &q)
216 {
217  real = q;
218 }
219 
220 template <class QUAT_TYPE> inline void
221 DualQuaternion<QUAT_TYPE>::SetDualPart(const Quaternion<QUAT_TYPE> &p)
222 {
223  dual = p;
224 }
225 
226 template <class QUAT_TYPE> inline Quaternion<QUAT_TYPE>
227 DualQuaternion<QUAT_TYPE>::GetRealPart() const
228 {
229  return real;
230 }
231 
232 template <class QUAT_TYPE> inline Quaternion<QUAT_TYPE>
233 DualQuaternion<QUAT_TYPE>::GetDualPart() const
234 {
235  return dual;
236 }
237 
238 template <class QUAT_TYPE> inline Vector<QUAT_TYPE>
239 DualQuaternion<QUAT_TYPE>::GetVector() const
240 {
241  Vector<QUAT_TYPE> vec(8);
242  for (register int i = 0; i < 4; i++) {
243  vec[i] = real[i];
244  vec[i+4] = dual[i];
245  }
246  return vec;
247 }
248 
249 template <class QUAT_TYPE> inline void
250 DualQuaternion<QUAT_TYPE>::
251 GetRigidMotion(RMatrixBase &rotation,
252  Vector3<QUAT_TYPE> &translation) const
253 {
254  Quaternion<QUAT_TYPE> quat;
255  GetRigidMotion(quat, translation);
256  rotation.SetFromQuaternion(Quaternion<ROTATION_MATRIX_TYPE>((ROTATION_MATRIX_TYPE)quat[0],
257  (ROTATION_MATRIX_TYPE)quat[1],
258  (ROTATION_MATRIX_TYPE)quat[2],
259  (ROTATION_MATRIX_TYPE)quat[3]));
260 }
261 
262 template <class QUAT_TYPE> inline void
263 DualQuaternion<QUAT_TYPE>::
264 SetFromRigidMotion(const RMatrixBase &rotation,
265  const Vector3<QUAT_TYPE> &translation)
266 {
267  Quaternion<ROTATION_MATRIX_TYPE> quat;
268  rotation.GetQuaternion(quat);
269  SetFromRigidMotion(Quaternion<QUAT_TYPE>((QUAT_TYPE)quat[0],
270  (QUAT_TYPE)quat[1],
271  (QUAT_TYPE)quat[2],
272  (QUAT_TYPE)quat[3]), translation);
273 }
Generated on Tue Feb 6 2018 02:01:31 for Basic Image AlgorithmS Library by   doxygen 1.8.5