Skip to content

Commit

Permalink
Base: ScLERP placement interpolation
Browse files Browse the repository at this point in the history
  • Loading branch information
@DeepSOIC @wwmayer
DeepSOIC authored and wwmayer committed Oct 12, 2019
1 parent 867a09f commit 63d0435
Show file tree
Hide file tree
Showing 6 changed files with 409 additions and 4 deletions.
3 changes: 3 additions & 0 deletions src/Base/CMakeLists.txt
View file
Expand Up @@ -238,6 +238,7 @@ SET(FreeCADBase_CPP_SRCS
CoordinateSystem.cpp
CoordinateSystemPyImp.cpp
Debugger.cpp
DualQuaternion.cpp
Exception.cpp
ExceptionFactory.cpp
Factory.cpp
Expand Down Expand Up @@ -306,6 +307,8 @@ SET(FreeCADBase_HPP_SRCS
Converter.h
CoordinateSystem.h
Debugger.h
DualNumber.h
DualQuaternion.h
Exception.h
ExceptionFactory.h
Factory.h
Expand Down
94 changes: 94 additions & 0 deletions src/Base/DualNumber.h
View file
@@ -0,0 +1,94 @@
/***************************************************************************
* Copyright (c) 2019 Viktor Titov (DeepSOIC) <vv.titov@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/

#ifndef FREECAD_BASE_DUAL_NUMBER_H
#define FREECAD_BASE_DUAL_NUMBER_H

#include <cmath>

namespace Base {


/**
* @brief Dual Numbers aer 2-part numbers like complex numbers, but different
* algebra. They are denoted as a + b*eps, where eps^2 = 0. eps, the nilpotent,
* is like imaginary unit of complex numbers. The neat utility of dual numbers
* is that if you use them instead of normal numbers in a function like sin(),
* derivative is implicitly calculated as a multiplier to the dual part.
*/
class DualNumber
{
public:
double re = 0.0;
double du = 0.0;
public:
DualNumber(){}
DualNumber(double re, double du = 0.0)
: re(re), du(du)
{}
DualNumber operator-() const {return DualNumber(-re,-du);}
};

inline DualNumber operator+(DualNumber a, DualNumber b){
return DualNumber(a.re + b.re, a.du + b.du);
}
inline DualNumber operator+(DualNumber a, double b){
return DualNumber(a.re + b, a.du);
}
inline DualNumber operator+(double a, DualNumber b){
return DualNumber(a + b.re, b.du);
}

inline DualNumber operator-(DualNumber a, DualNumber b){
return DualNumber(a.re - b.re, a.du - b.du);
}
inline DualNumber operator-(DualNumber a, double b){
return DualNumber(a.re - b, a.du);
}
inline DualNumber operator-(double a, DualNumber b){
return DualNumber(a - b.re, -b.du);
}

inline DualNumber operator*(DualNumber a, DualNumber b){
return DualNumber(a.re * b.re, a.re * b.du + a.du * b.re);
}
inline DualNumber operator*(double a, DualNumber b){
return DualNumber(a * b.re, a * b.du);
}
inline DualNumber operator*(DualNumber a, double b){
return DualNumber(a.re * b, a.du * b);
}

inline DualNumber operator/(DualNumber a, DualNumber b){
return DualNumber(a.re / b.re, (a.du * b.re - a.re * b.du) / (b.re * b.re));
}
inline DualNumber operator/(DualNumber a, double b){
return DualNumber(a.re / b, a.du / b);
}

inline DualNumber pow(DualNumber a, double pw){
return Base::DualNumber(std::pow(a.re, pw), pw * std::pow(a.re, pw - 1.0) * a.du);
}
} //namespace


#endif
166 changes: 166 additions & 0 deletions src/Base/DualQuaternion.cpp
View file
@@ -0,0 +1,166 @@
/***************************************************************************
* Copyright (c) 2019 Viktor Titov (DeepSOIC) <vv.titov@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/

#include "PreCompiled.h"

#include "DualQuaternion.h"

#include "cassert"

Base::DualQuat Base::operator+(Base::DualQuat a, Base::DualQuat b)
{
return DualQuat(
a.x + b.x,
a.y + b.y,
a.z + b.z,
a.w + b.w
);
}

Base::DualQuat Base::operator-(Base::DualQuat a, Base::DualQuat b)
{
return DualQuat(
a.x - b.x,
a.y - b.y,
a.z - b.z,
a.w - b.w
);
}

Base::DualQuat Base::operator*(Base::DualQuat a, Base::DualQuat b)
{
return DualQuat(
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
);
}

Base::DualQuat Base::operator*(Base::DualQuat a, double b)
{
return DualQuat(
a.x * b,
a.y * b,
a.z * b,
a.w * b
);
}

Base::DualQuat Base::operator*(double a, Base::DualQuat b)
{
return DualQuat(
b.x * a,
b.y * a,
b.z * a,
b.w * a
);
}

Base::DualQuat Base::operator*(Base::DualQuat a, Base::DualNumber b)
{
return DualQuat(
a.x * b,
a.y * b,
a.z * b,
a.w * b
);
}

Base::DualQuat Base::operator*(Base::DualNumber a, Base::DualQuat b)
{
return DualQuat(
b.x * a,
b.y * a,
b.z * a,
b.w * a
);
}

Base::DualQuat::DualQuat(Base::DualQuat re, Base::DualQuat du)
: x(re.x.re, du.x.re),
y(re.y.re, du.y.re),
z(re.z.re, du.z.re),
w(re.w.re, du.w.re)
{
assert(re.dual().length() < 1e-12);
assert(du.dual().length() < 1e-12);
}

double Base::DualQuat::dot(Base::DualQuat a, Base::DualQuat b)
{
return a.x.re * b.x.re +
a.y.re * b.y.re +
a.z.re * b.z.re +
a.w.re * b.w.re ;
}

Base::DualQuat Base::DualQuat::pow(double t, bool shorten) const
{
/* implemented after "Dual-Quaternions: From Classical Mechanics to
* Computer Graphics and Beyond" by Ben Kenwright www.xbdev.net
* bkenwright@xbdev.net
* http://www.xbdev.net/misc_demos/demos/dual_quaternions_beyond/paper.pdf
*
* There are some differences:
*
* * Special handling of no-rotation situation (because normalization
* multiplier becomes infinite in this situation, breaking the algorithm).
*
* * Dual quaternions are implemented as a collection of dual numbers,
* rather than a collection of two quaternions like it is done in suggested
* inplementation in the paper.
*
* * acos replaced with atan2 for improved angle accuracy for small angles
*
* */
double le = this->vec().length();
if (le < 1e-12) {
//special case of no rotation. Interpolate position
return DualQuat(this->real(), this->dual()*t);
}

double normmult = 1.0/le;

DualQuat self = *this;
if (shorten){
if (dot(self, identity()) < -1e-12){ //using negative tolerance instead of zero, for stability in situations the choice is ambiguous (180-degree rotations)
self = -self;
}
}

//to screw coordinates
double theta = self.theta();
double pitch = -2.0 * self.w.du * normmult;
DualQuat l = self.real().vec() * normmult; //abusing DualQuat to store vectors. Very handy in this case.
DualQuat m = (self.dual().vec() - pitch/2*cos(theta/2)*l)*normmult;

//interpolate
theta *= t;
pitch *= t;

//back to quaternion
return DualQuat(
l * sin(theta/2) + DualQuat(0,0,0,cos(theta/2)),
m * sin(theta/2) + pitch / 2 * cos(theta/2) * l + DualQuat(0,0,0,-pitch/2*sin(theta/2))
);
}
108 changes: 108 additions & 0 deletions src/Base/DualQuaternion.h
View file
@@ -0,0 +1,108 @@
/***************************************************************************
* Copyright (c) 2019 Viktor Titov (DeepSOIC) <vv.titov@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/

#ifndef FREECAD_BASE_DUAL_QUATERNION_H
#define FREECAD_BASE_DUAL_QUATERNION_H

#include "DualNumber.h"
//#include <Console.h> //DEBUG

namespace Base {

/**
* @brief The DualQuat class represents a dual quaternion, as a quaternion of
* dual number components. Dual quaternions are useful for placement
* interpolation, see pow method.
*
* Rotation is stored as non-dual part of DualQ. Translation is encoded into
* dual part of DualQuat:
* DualQuat.dual() = 0.5 * t * r,
* where t is quaternion with x,y,z of translation and w of 0, and r is the
* rotation quaternion.
*/
class BaseExport DualQuat {
public:
DualNumber x;
DualNumber y;
DualNumber z;
DualNumber w;
public:
///default constructor: init with zeros
DualQuat(){}
DualQuat(DualNumber x, DualNumber y, DualNumber z, DualNumber w)
: x(x), y(y), z(z), w(w) {}
DualQuat(double x,double y,double z,double w,double dx,double dy,double dz,double dw)
: x(x, dx), y(y, dy), z(z, dz), w(w, dw) {}
DualQuat(double x,double y,double z,double w)
: x(x), y(y), z(z), w(w) {}

///Builds a dual quaternion from real and dual parts provided as pure real quaternions
DualQuat(DualQuat re, DualQuat du);

///returns dual quaternion for identity placement
static DualQuat identity() {return DualQuat(0.0, 0.0, 0.0, 1.0);}

///return a copy with dual part zeroed out
DualQuat real() const {return DualQuat(x.re, y.re, z.re, w.re);}

///return a real-only quaternion made from dual part of this quaternion.
DualQuat dual() const {return DualQuat(x.du, y.du, z.du, w.du);}

///conjugate
DualQuat conj() const {return DualQuat(-x, -y, -z, w);}

///return vector part (with scalar part zeroed out)
DualQuat vec() const {return DualQuat(x,y,z,0.0);}

///magnitude of the quaternion
double length() const {return sqrt(x.re*x.re + y.re*y.re + z.re*z.re + w.re*w.re);}

///angle of rotation represented by this quaternion, in radians
double theta() const {return 2.0 * atan2(vec().length(), w.re);}

///dot product between real (rotation) parts of two dual quaternions (to determine if one of them should be negated for shortest interpolation)
static double dot(DualQuat a, DualQuat b);

///ScLERP. t=0.0 returns identity, t=1.0 returns this. t can also be outside of 0..1 bounds.
DualQuat pow(double t, bool shorten = true) const;

DualQuat operator-() const {return DualQuat(-x, -y, -z, -w);}

//DEBUG
//void print() const {
// Console().Log("%f, %f, %f, %f; %f, %f, %f, %f", x.re,y.re,z.re,w.re, x.du,y.du,z.du, w.du);
//}
};

DualQuat operator+(DualQuat a, DualQuat b);
DualQuat operator-(DualQuat a, DualQuat b);
DualQuat operator*(DualQuat a, DualQuat b);

DualQuat operator*(DualQuat a, double b);
DualQuat operator*(double a, DualQuat b);
DualQuat operator*(DualQuat a, DualNumber b);
DualQuat operator*(DualNumber a, DualQuat b);


} //namespace

#endif

0 comments on commit 63d0435

Please sign in to comment.