dual_quaternion.h
Go to the documentation of this file.
1 /********************************************************************************
2 Copyright (c) 2015, TRACLabs, Inc.
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6  are permitted provided that the following conditions are met:
7 
8  1. Redistributions of source code must retain the above copyright notice,
9  this list of conditions and the following disclaimer.
10 
11  2. Redistributions in binary form must reproduce the above copyright notice,
12  this list of conditions and the following disclaimer in the documentation
13  and/or other materials provided with the distribution.
14 
15  3. Neither the name of the copyright holder nor the names of its contributors
16  may be used to endorse or promote products derived from this software
17  without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
23 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24 BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
26 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
27 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
28 OF THE POSSIBILITY OF SUCH DAMAGE.
29 ********************************************************************************/
30 
31 #ifndef DUAL_QUATERNION_HPP
32 #define DUAL_QUATERNION_HPP
33 
34 #include "math3d.h"
35 
36 using math3d::point3d;
37 using math3d::matrix;
38 using math3d::quaternion;
39 
40 
41 template<typename T> inline int sign(T v)
42 {
43  return (v < 0) ? -1 : 1;
44 }
45 
46 void set_quaternion_matrix(matrix<double>&M, const quaternion<double>& q, int i = 0, int j = 0, double w = 1.0)
47 {
48  //{{a, -b, -c, -d}, {b, a, -d, c}, {c, d, a, -b}, {d, -c, b, a}}
49  M(i, j) = w * q.w;
50  M(i, j + 1) = -w * q.i;
51  M(i, j + 2) = -w * q.j;
52  M(i, j + 3) = -w * q.k;
53  M(i + 1, j) = w * q.i;
54  M(i + 1, j + 1) = w * q.w;
55  M(i + 1, j + 2) = -w * q.k;
56  M(i + 1, j + 3) = w * q.j;
57  M(i + 2, j) = w * q.j;
58  M(i + 2, j + 1) = w * q.k;
59  M(i + 2, j + 2) = w * q.w;
60  M(i + 2, j + 3) = -w * q.i;
61  M(i + 3, j) = w * q.k;
62  M(i + 3, j + 1) = -w * q.j;
63  M(i + 3, j + 2) = w * q.i;
64  M(i + 3, j + 3) = w * q.w;
65 }
66 
68 {
70 
71  dual_quaternion(double v = 1.0) : R(v), tR_2(0) {}
72 
73  static constexpr double dq_epsilon = 1e-8;
74 
76  {
77  dual_quaternion result;
78  result.R = r;
79  result.tR_2 = (quaternion<double>::convert(t) * r) *= 0.5;
80  return result;
81  }
82  static dual_quaternion convert(const double* p)
83  {
84  dual_quaternion result;
85  result.R = quaternion<double>::convert(p);
86  result.tR_2 = quaternion<double>::convert(p + 4);
87  return result;
88  }
89 
91  {
92  double n = norm(R) * sign(R.w);
93  R *= 1.0 / n;
94  tR_2 *= 1.0 / n;
95  double d = dot(R, tR_2);
96  //tR_2 += (-d)*R;
97  quaternion<double> r2 = R;
98  r2 *= -d;
99  tR_2 += r2;
100  return *this;
101  }
102 
104  {
105  quaternion<double> t = tR_2 * ~R;
106  point3d result;
107  result.x = 2 * t.i;
108  result.y = 2 * t.j;
109  result.z = 2 * t.k;
110  return result;
111  }
112 
113  void to_vector(double* p)
114  {
115  R.to_vector(p);
116  tR_2.to_vector(p + 4);
117  }
118 
120  {
121  R += a.R;
122  tR_2 += a.tR_2;
123  return *this;
124  }
125 
127  {
128  R *= a;
129  tR_2 *= a;
130  return *this;
131  }
132 
133  dual_quaternion& log() //computes log map tangent at identity
134  {
135  //assumes qual_quaternion is unitary
136  const double h0 = std::acos(R.w);
137  if (h0 * h0 < dq_epsilon) //small angle approximation: sin(h0)=h0, cos(h0)=1
138  {
139  R.w = 0.0;
140  R *= 0.5;
141  tR_2.w = 0.0;
142  tR_2 *= 0.5;
143  }
144  else
145  {
146  R.w = 0.0;
147  const double ish0 = 1.0 / norm(R);
148  //R *= ish0;
149  math3d::normalize(R); //R=s0
150  const double he = -tR_2.w * ish0;
151  tR_2.w = 0.0;
152 
153  quaternion<double> Rp(R);
154  Rp *= -dot(R, tR_2) / dot(R, R);
155  tR_2 += Rp;
156  tR_2 *= ish0; //tR_2=se
157 
158  tR_2 *= h0;
159  Rp = R;
160  Rp *= he;
161  tR_2 += Rp;
162  tR_2 *= 0.5;
163  R *= h0 * 0.5;
164  }
165 
166  return *this;
167  }
168 
169  dual_quaternion& exp() //computes exp map tangent at identity
170  {
171  //assumes qual_quaternion is on tangent space
172  const double h0 = 2.0 * norm(R);
173 
174  if (h0 * h0 < dq_epsilon) //small angle approximation: sin(h0)=h0, cos(h0)=1
175  {
176  R *= 2.0;
177  R.w = 1.0;
178  tR_2 *= 2.0;
179  //normalize();
180  }
181  else
182  {
183  const double he = 4.0 * math3d::dot(tR_2, R) / h0;
184  const double sh0 = sin(h0), ch0 = cos(h0);
185  quaternion<double> Rp(R);
186  Rp *= -dot(R, tR_2) / dot(R, R);
187  tR_2 += Rp;
188  tR_2 *= 2.0 / h0; //tR_2=se
189 
190 
191  tR_2 *= sh0;
192  Rp = R;
193  Rp *= he * ch0 * 2.0 / h0;
194  tR_2 += Rp;
195  tR_2.w = -he * sh0;
196 
197  R *= sh0 * 2.0 / h0;
198  R.w = ch0;
199  }
200  normalize();
201  return *this;
202  }
203 };
204 
205 
207 {
208  dual_quaternion result;
209  result.R = a.R * b.R;
210  result.tR_2 = a.R * b.tR_2 + a.tR_2 * b.R;
211  return result;
212 }
213 
215 {
216  dual_quaternion result;
217  result.R = ~a.R;
218  result.tR_2 = ((~a.tR_2) *= -1);
219  return result;
220 }
221 
223 {
224  dual_quaternion result;
225  result.R = ~a.R;
226  result.tR_2 = ~a.tR_2;
227  return result;
228 }
229 
230 double dot(const dual_quaternion& a, const dual_quaternion& b)
231 {
232  return dot(a.R, b.R) + dot(a.tR_2, b.tR_2);
233 }
234 
235 void set_dual_quaternion_matrix(matrix<double>& M, const dual_quaternion& dq, int i = 0, int j = 0, double w = 1.0)
236 {
237  set_quaternion_matrix(M, dq.R, i, j, w);
238  M(i, j + 4) = M(i, j + 5) = M(i, j + 6) = M(i, j + 7) = 0;
239  M(i + 1, j + 4) = M(i + 1, j + 5) = M(i + 1, j + 6) = M(i + 1, j + 7) = 0;
240  M(i + 2, j + 4) = M(i + 2, j + 5) = M(i + 2, j + 6) = M(i + 2, j + 7) = 0;
241  M(i + 3, j + 4) = M(i + 3, j + 5) = M(i + 3, j + 6) = M(i + 3, j + 7) = 0;
242  set_quaternion_matrix(M, dq.tR_2, i + 4, j, w);
243  set_quaternion_matrix(M, dq.R, i + 4, j + 4, w);
244 }
245 
247 {
248  return a.log();
249 }
251 {
252  return a.exp();
253 }
254 
255 
256 std::ostream& operator << (std::ostream& out, const dual_quaternion& dq)
257 {
258  return out << "( " << dq.R.w << ", " << dq.R.i << ", " << dq.R.j << ", " << dq.R.k << ", "
259  << dq.tR_2.w << ", " << dq.tR_2.i << ", " << dq.tR_2.j << ", " << dq.tR_2.k << " )";
260 }
261 
262 #endif
d
double dot(const dual_quaternion &a, const dual_quaternion &b)
dual_quaternion(double v=1.0)
vec3d< double > point3d
Definition: math3d.h:205
dual_quaternion operator!(const dual_quaternion &a)
std::ostream & operator<<(std::ostream &out, const dual_quaternion &dq)
int sign(T v)
static dual_quaternion rigid_transformation(const quaternion< double > &r, const point3d &t)
void to_vector(T *p) const
Definition: math3d.h:621
dual_quaternion operator*(const dual_quaternion &a, const dual_quaternion &b)
point3d get_translation()
quaternion< double > tR_2
T norm(const quaternion< T > &a)
Definition: math3d.h:656
quaternion< double > R
T dot(const quaternion< T > &a, const quaternion< T > &b)
Definition: math3d.h:650
dual_quaternion & normalize()
void set_dual_quaternion_matrix(matrix< double > &M, const dual_quaternion &dq, int i=0, int j=0, double w=1.0)
dual_quaternion & operator*=(double a)
dual_quaternion operator~(const dual_quaternion &a)
dual_quaternion & exp()
dual_quaternion & log()
void normalize(quaternion< T > &q)
Definition: math3d.h:689
static dual_quaternion convert(const double *p)
void to_vector(double *p)
dual_quaternion & operator+=(const dual_quaternion &a)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
static constexpr double dq_epsilon
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
void set_quaternion_matrix(matrix< double > &M, const quaternion< double > &q, int i=0, int j=0, double w=1.0)


trac_ik_lib
Author(s): Patrick Beeson, Barrett Ames
autogenerated on Sat Sep 19 2020 03:40:57