1 #ifndef GRL_DUAL_QUATERNION 2 #define GRL_DUAL_QUATERNION 6 template<
typename Vector,
typename Quaternion>
9 vD.w() = -0.5*( vT(0)*q.x() + vT(1)*q.y() + vT(2)*q.z());
10 vD.x() = 0.5*( vT(0)*q.w() + vT(1)*q.z() - vT(2)*q.y());
11 vD.y() = 0.5*(-vT(0)*q.z() + vT(1)*q.w() + vT(2)*q.x());
12 vD.z() = 0.5*( vT(0)*q.y() - vT(1)*q.x() + vT(2)*q.w());
28 #define EPS(value_t) (std::numeric_limits<value_t>::epsilon()) 30 template <
class value_t>
35 quat(
bool e=1) : w(e), x(0), y(0), z(0) {}
40 value_t z_) : w(w_), x(x_) , y(y_), z(z_) {}
43 std::cout <<
"(" << w <<
"," << x
44 <<
"," << y <<
"," << z
50 y+other.
y, z+other.
z);
54 w += other.
w; x += other.
x; y += other.
y; z += other.
z;
60 y-other.
y, z-other.
z);
64 w -= other.
w; x -= other.
x; y -= other.
y; z -= other.
z;
70 w*other.
x+other.
w*x+y*other.
z-z*other.
y,
71 w*other.
y+other.
w*y+z*other.
x-x*other.
z,
72 w*other.
z+other.
w*z+x*other.
y-y*other.
x);
76 *
this = (*this)*other;
91 const value_t rho = std::sqrt(w*w+x*x+y*y+z*z);
100 const value_t rho2 = w*w+x*x+y*y+z*
z;
106 assert(w*w <
EPS(value_t));
108 value_t dst = std::sqrt(x*x+y*y+z*z);
109 value_t fac = std::sin(dst)/dst;
120 while (pow.
dot(pow) > eps) {
132 if ((w*w-1)*(w*w-1) <
EPS(value_t))
135 const value_t inv = 1.0/std::sqrt(x*x+y*y+z*z);
136 const value_t fac = std::acos(w)*inv;
142 return w*other.
w+x*other.
x+y*other.
y+z*other.
z;
150 const auto& generator =
log();
151 return generator.dot(generator);
156 if (
dot(other) < 0.0) {
157 const auto& difference = (*this)+other;
158 return difference.
dot(difference);
161 const auto& difference = (*this)-other;
162 return difference.
dot(difference);
166 const auto& difference = ((*this)*(other.
C())).log();
167 return difference.dot(difference);
171 const value_t residue = w*w+x*x+y*y+z*z-1.0;
172 return residue*residue <
EPS(value_t);
176 template <
class value_t>
182 W(0), X(0), Y(0), Z(0) {};
191 const value_t Z_) : w(w_), x(x_), y(y_), z(z_),
192 W(W_), X(X_), Y(Y_), Z(Z_) {}
196 y(real.y), z(real.z),
197 W(dual.w), X(dual.x),
198 Y(dual.y), Z(dual.z) {}
209 std::cout <<
"(" << w <<
"," << x
210 <<
"," << y <<
"," << z
211 <<
"," << W <<
"," << X
212 <<
"," << Y <<
"," << Z
218 y+other.
y, z+other.
z,
219 W+other.
W, X+other.
X,
220 Y+other.
Y, Z+other.
Z);
224 w += other.
w; x += other.
x; y += other.
y; z += other.
z;
225 W += other.
W; X += other.
X; Y += other.
Y; Z += other.
Z;
231 y-other.
y, z-other.
z,
232 W-other.
W, X-other.
X,
233 Y-other.
Y, Z-other.
Z);
237 w -= other.
w; x -= other.
x; y -= other.
y; z -= other.
z;
238 W -= other.
W; X -= other.
X; Y -= other.
Y; Z -= other.
Z;
245 const auto& a = real();
246 const auto& A = dual();
247 const auto& b = other.
real();
248 const auto& B = other.
dual();
254 *
this = (*this)*other;
274 const value_t qq = w*w+x*x+y*y+z*z+
EPS(value_t);
275 const value_t qQ = w*W+x*X+y*Y+z*Z;
276 const value_t invqq = 1.0/qq;
277 const value_t invsq = 1.0/std::sqrt(qq);
278 const value_t alpha = qQ*invqq*invsq;
282 W*invsq-w*alpha, X*invsq-x*alpha,
283 Y*invsq-y*alpha, Z*invsq-z*alpha);
296 const value_t qq = w*w+x*x+y*y+z*
z;
297 const value_t qQ = w*W+x*X+y*Y+z*Z;
298 const value_t invqq = 1.0/qq;
299 const value_t alpha = 2.0*qQ*invqq*invqq;
303 W*invqq-w*alpha, -X*invqq-x*alpha,
304 -Y*invqq-y*alpha, -Z*invqq-z*alpha);
309 assert(w*w <
EPS(value_t) && W*W <
EPS(value_t));
311 if (x*x+y*y+z*z <
EPS(value_t))
314 const value_t theta = 2.0*std::sqrt(x*x+y*y+z*z);
315 const value_t invvv = 2.0/theta;
316 const value_t lx = x*invvv;
317 const value_t ly = y*invvv;
318 const value_t lz = z*invvv;
320 const value_t pitch = 2.0*(lx*X+ly*Y+lz*Z);
321 const value_t mx = (2.0*X-pitch*lx)/theta;
322 const value_t my = (2.0*Y-pitch*ly)/theta;
323 const value_t mz = (2.0*Z-pitch*lz)/theta;
325 assert((lx*mx+ly*my+lz*mz)*(lx*mx+ly*my+lz*mz) <
EPS(value_t));
327 const value_t cost2 = std::cos(0.5*theta);
328 const value_t sint2 = std::sin(0.5*theta);
329 const value_t alpha = 0.5*pitch*cost2;
348 while (pow.
dot(pow) > eps) {
360 if ((w*w-1)*(w*w-1) <
EPS(value_t))
363 const value_t theta = 2.0*std::acos(w);
364 const value_t invvv = 0.5/std::sqrt(x*x+y*y+z*z);
365 const value_t pitch = -4.0*W*invvv;
366 const value_t alpha = pitch*
w;
368 const value_t lx = x*invvv;
369 const value_t ly = y*invvv;
370 const value_t lz = z*invvv;
371 const value_t mx = (X-lx*alpha)*invvv;
372 const value_t my = (Y-ly*alpha)*invvv;
373 const value_t mz = (Z-lz*alpha)*invvv;
375 assert((lx*mx+ly*my+lz*mz)*(lx*mx+ly*my+lz*mz) <
EPS(value_t));
380 0, (pitch*lx+theta*mx),
382 (pitch*lz+theta*mz));
390 const auto& lower = real().log();
391 const auto& upper = dual()*real().C();
397 return w*other.
w+x*other.
x+y*other.
y+z*other.
z+
398 W*other.
W+X*other.
X+Y*other.
Y+Z*other.
Z;
406 const auto& generator =
log();
407 return generator.dot(generator);
411 const auto& generator = fakelog();
412 return generator.dot(generator);
418 if (
dot(other) < 0.0) {
419 const auto& difference = (*this)+other;
420 return difference.
dot(difference);
423 const auto& difference = (*this)-other;
424 return difference.
dot(difference);
428 const auto& difference = ((*this)*(other.
C())).log();
429 return difference.dot(difference);
433 const auto& difference = ((*this)*(other.
C())).fakelog();
434 return difference.dot(difference);
439 const value_t residue0 = w*w+x*x+y*y+z*z-1.0;
440 const value_t residue1 = w*W+x*X+y*Y+z*Z;
442 return residue0*residue0 <
EPS(value_t) &&
443 residue1*residue1 <
EPS(value_t);
451 template <
class value_t>
456 for (
size_t i = 0; i < quats.size(); i++)
462 template <
class value_t>
467 auto logmean = [&]() {
469 for (
size_t i = 0; i < quats.size(); i++)
470 avg += (b.
C()*quats[i]).
log();
471 return avg/quats.size();
475 auto norm = x.dot(x);
480 auto xnew = logmean();
482 const auto newnorm = xnew.dot(xnew);
483 if(norm < newnorm || newnorm <
EPS(value_t))
494 template <
class value_t>
496 const std::vector<value_t>& weights) {
498 assert(quats.size() == weights.size());
502 for (
size_t i = 0; i < quats.size(); i++)
503 result += quats[i]*weights[i];
508 template <
class value_t>
510 const std::vector<value_t>& weights) {
513 assert(quats.size() == weights.size());
517 auto logmean = [&]() {
519 for (
size_t i = 0; i < quats.size(); i++)
520 avg += ((b.C())*quats[i]).
log()*weights[i];
525 auto norm = x.dot(x);
530 auto xnew = logmean();
532 const auto newnorm = xnew.dot(xnew);
533 if(norm < newnorm || newnorm <
EPS(value_t))
544 template <
class value_t>
549 for (
size_t i = 0; i < quats.size(); i++)
555 template <
class value_t>
560 auto logmean = [&]() {
562 for (
size_t i = 0; i < quats.size(); i++)
563 avg += (b.
C()*quats[i]).
log();
564 return avg/quats.size();
568 auto norm = x.dot(x);
573 auto xnew = logmean();
575 const auto newnorm = xnew.dot(xnew);
576 if(norm < newnorm || newnorm <
EPS(value_t))
589 template <
class value_t>
591 const std::vector<value_t>& weights) {
593 assert(quats.size() == weights.size());
597 for (
size_t i = 0; i < quats.size(); i++)
598 result += quats[i]*weights[i];
603 template <
class value_t>
605 const std::vector<value_t>& weights) {
608 assert(quats.size() == weights.size());
612 auto logmean = [&]() {
614 for (
size_t i = 0; i < quats.size(); i++)
615 avg += ((b.C())*quats[i]).
log()*weights[i];
620 auto norm = x.dot(x);
625 auto xnew = logmean();
627 const auto newnorm = xnew.dot(xnew);
628 if(norm < newnorm || newnorm <
EPS(value_t))
dualquat< value_t > numexp(value_t eps=(std::numeric_limits< value_t >::epsilon())) const
dualquat< value_t > D() const
dualquat< value_t > & operator+=(const dualquat< value_t > &other)
quat< value_t > & operator-=(const quat< value_t > &other)
value_t eucdist(const quat< value_t > &other) const
quat< value_t > log() const
dualquat< value_t > N() const
dualquat< value_t > I() const
quat< value_t > operator/(const value_t alpha) const
Quaternion TranslationToDual(const Vector &vT)
quat< value_t > & operator*=(const quat< value_t > &other)
quat< value_t > operator-(const quat< value_t > &other) const
value_t dot(const dualquat< value_t > other) const
dualquat< value_t > exp() const
dualquat< value_t > operator+(const dualquat< value_t > &other) const
value_t dot(const quat< value_t > other) const
quat< value_t > & operator+=(const quat< value_t > &other)
quat< value_t > C() const
quat< value_t > numexp(value_t eps=(std::numeric_limits< value_t >::epsilon())) const
quat< value_t > I() const
quat< value_t > QIA(const std::vector< quat< value_t >> &quats)
dualquat< value_t > C() const
value_t logdist(const quat< value_t > &other) const
dualquat< value_t > operator*(const dualquat< value_t > &other) const
dualquat< value_t > fakelog() const
dualquat< value_t > log() const
dualquat< value_t > DIA(const std::vector< dualquat< value_t >> &quats)
value_t kindist(const dualquat< value_t > &other) const
value_t eucdist(const dualquat< value_t > &other) const
dualquat< value_t > & operator-=(const dualquat< value_t > &other)
quat< value_t > operator*(const quat< value_t > &other) const
dualquat< value_t > DLA(const std::vector< dualquat< value_t >> &quats)
quat< value_t > dual() const
value_t logdist(const dualquat< value_t > &other) const
dualquat< value_t > operator*(const value_t alpha) const
dualquat< value_t > DIB(const std::vector< dualquat< value_t >> &quats, const std::vector< value_t > &weights)
quat< value_t > exp() const
dualquat< value_t > operator-(const dualquat< value_t > &other) const
dualquat< value_t > operator/(const value_t alpha) const
quat< value_t > N() const
quat< value_t > QLA(const std::vector< quat< value_t >> &quats)
quat< value_t > QIB(const std::vector< quat< value_t >> &quats, const std::vector< value_t > &weights)
quat< value_t > operator+(const quat< value_t > &other) const
dualquat(const quat< value_t > &real, const quat< value_t > &dual)
quat< value_t > real() const
dualquat< value_t > DLB(const std::vector< dualquat< value_t >> &quats, const std::vector< value_t > &weights)
dualquat(const value_t w_, const value_t x_, const value_t y_, const value_t z_, const value_t W_, const value_t X_, const value_t Y_, const value_t Z_)
quat(value_t w_, value_t x_, value_t y_, value_t z_)
quat< value_t > QLB(const std::vector< quat< value_t >> &quats, const std::vector< value_t > &weights)
quat< value_t > operator*(const value_t alpha) const
dualquat< value_t > & operator*=(const dualquat< value_t > &other)