kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Base: ScLERP placement interpolation

Browse Source
This commit is contained in:
DeepSOIC
2019-10-11 13:21:12 +03:00
committed by wwmayer
parent 15c6b01237
commit 6a9e4b710b
6 changed files with 409 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/Base/CMakeLists.txt
View File

@@ -238,6 +238,7 @@ SET(FreeCADBase_CPP_SRCS
CoordinateSystem.cpp
CoordinateSystemPyImp.cpp
Debugger.cpp
DualQuaternion.cpp
Exception.cpp
ExceptionFactory.cpp
Factory.cpp
@@ -306,6 +307,8 @@ SET(FreeCADBase_HPP_SRCS
Converter.h
CoordinateSystem.h
Debugger.h
DualNumber.h
DualQuaternion.h
Exception.h
ExceptionFactory.h
Factory.h

94
src/Base/DualNumber.h Normal file
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
src/Base/DualQuaternion.cpp Normal file
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
src/Base/DualQuaternion.h Normal file
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

30
src/Base/Placement.cpp
View File

@@ -20,14 +20,13 @@
* *
***************************************************************************/
#include "PreCompiled.h"
#ifndef _PreComp_
#endif
#include "Placement.h"
#include "Rotation.h"
#include "DualQuaternion.h"
using namespace Base;
@@ -61,6 +60,13 @@ Placement::Placement(const Vector3d& Pos, const Rotation &Rot, const Vector3d& C
this->_rot = Rot;
}
Placement Placement::fromDualQuaternion(DualQuat qq)
{
Rotation rot(qq.x.re, qq.y.re, qq.z.re, qq.w.re);
DualQuat mvq = 2 * qq.dual() * qq.real().conj();
return Placement(Vector3d(mvq.x.re,mvq.y.re, mvq.z.re), rot);
}
Base::Matrix4D Placement::toMatrix(void) const
{
Base::Matrix4D matrix;
@@ -79,6 +85,15 @@ void Placement::fromMatrix(const Base::Matrix4D& matrix)
this->_pos.z = matrix[2][3];
}
DualQuat Placement::toDualQuaternion() const
{
DualQuat posqq(_pos.x, _pos.y, _pos.z, 0.0);
DualQuat rotqq;
_rot.getValue(rotqq.x.re, rotqq.y.re, rotqq.z.re, rotqq.w.re);
DualQuat ret (rotqq, 0.5 * posqq * rotqq);
return ret;
}
bool Placement::isIdentity() const
{
Base::Vector3d nullvec(0,0,0);
@@ -138,6 +153,11 @@ Placement& Placement::operator = (const Placement& New)
return *this;
}
Placement Placement::pow(double t, bool shorten) const
{
return Placement::fromDualQuaternion(this->toDualQuaternion().pow(t, shorten));
}
void Placement::multVec(const Vector3d & src, Vector3d & dst) const
{
this->_rot.multVec(src, dst);
@@ -150,3 +170,9 @@ Placement Placement::slerp(const Placement & p0, const Placement & p1, double t)
Vector3d pos = p0.getPosition() * (1.0-t) + p1.getPosition() * t;
return Placement(pos, rot);
}
Placement Placement::sclerp(const Placement& p0, const Placement& p1, double t, bool shorten)
{
Placement trf = p0.inverse() * p1;
return p0 * trf.pow(t, shorten);
}

12
src/Base/Placement.h
View File

@@ -20,7 +20,6 @@
* *
***************************************************************************/
#ifndef BASE_PLACEMENT_H
#define BASE_PLACEMENT_H
@@ -29,9 +28,9 @@
#include "Matrix.h"
namespace Base {
class DualQuat;
/**
* The Placement class.
@@ -45,11 +44,18 @@ public:
Placement(const Base::Matrix4D& matrix);
Placement(const Vector3d& Pos, const Rotation &Rot);
Placement(const Vector3d& Pos, const Rotation &Rot, const Vector3d& Cnt);
/** specialty constructors */
//@{
static Placement fromDualQuaternion(DualQuat qq);
//@}
/// Destruction
~Placement () {}
Matrix4D toMatrix(void) const;
void fromMatrix(const Matrix4D& m);
DualQuat toDualQuaternion() const;
const Vector3d& getPosition(void) const {return _pos;}
const Rotation& getRotation(void) const {return _rot;}
void setPosition(const Vector3d& Pos){_pos=Pos;}
@@ -67,11 +73,13 @@ public:
bool operator == (const Placement&) const;
bool operator != (const Placement&) const;
Placement& operator = (const Placement&);
Placement pow(double t, bool shorten = true) const;
void multVec(const Vector3d & src, Vector3d & dst) const;
//@}
static Placement slerp(const Placement & p0, const Placement & p1, double t);
static Placement sclerp(const Placement & p0, const Placement & p1, double t, bool shorten = true);
protected:
Vector3<double> _pos;