From 63d0435a95127d8e47d5c6b3f415d5c36450ed33 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Fri, 11 Oct 2019 13:21:12 +0300 Subject: [PATCH] Base: ScLERP placement interpolation --- src/Base/CMakeLists.txt | 3 + src/Base/DualNumber.h | 94 ++++++++++++++++++++ src/Base/DualQuaternion.cpp | 166 ++++++++++++++++++++++++++++++++++++ src/Base/DualQuaternion.h | 108 +++++++++++++++++++++++ src/Base/Placement.cpp | 30 ++++++- src/Base/Placement.h | 12 ++- 6 files changed, 409 insertions(+), 4 deletions(-) create mode 100644 src/Base/DualNumber.h create mode 100644 src/Base/DualQuaternion.cpp create mode 100644 src/Base/DualQuaternion.h diff --git a/src/Base/CMakeLists.txt b/src/Base/CMakeLists.txt index dc982dfd2034..790262ac9393 100644 --- a/src/Base/CMakeLists.txt +++ b/src/Base/CMakeLists.txt @@ -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 diff --git a/src/Base/DualNumber.h b/src/Base/DualNumber.h new file mode 100644 index 000000000000..48697e060f1c --- /dev/null +++ b/src/Base/DualNumber.h @@ -0,0 +1,94 @@ +/*************************************************************************** + * Copyright (c) 2019 Viktor Titov (DeepSOIC) * + * * + * 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 + +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 diff --git a/src/Base/DualQuaternion.cpp b/src/Base/DualQuaternion.cpp new file mode 100644 index 000000000000..c8c5966f8da6 --- /dev/null +++ b/src/Base/DualQuaternion.cpp @@ -0,0 +1,166 @@ +/*************************************************************************** + * Copyright (c) 2019 Viktor Titov (DeepSOIC) * + * * + * 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)) + ); +} diff --git a/src/Base/DualQuaternion.h b/src/Base/DualQuaternion.h new file mode 100644 index 000000000000..1959cc787d7f --- /dev/null +++ b/src/Base/DualQuaternion.h @@ -0,0 +1,108 @@ +/*************************************************************************** + * Copyright (c) 2019 Viktor Titov (DeepSOIC) * + * * + * 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 //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 diff --git a/src/Base/Placement.cpp b/src/Base/Placement.cpp index 361367171a68..7aa706699956 100644 --- a/src/Base/Placement.cpp +++ b/src/Base/Placement.cpp @@ -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); +} diff --git a/src/Base/Placement.h b/src/Base/Placement.h index 207e38aceb79..35661728ea06 100644 --- a/src/Base/Placement.h +++ b/src/Base/Placement.h @@ -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 @@ class BaseExport Placement 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 @@ class BaseExport Placement 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 _pos;