FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2019 Viktor Titov (DeepSOIC) <vv.titov@gmail.com>       *
3
 *                                                                         *
4
 *   This file is part of the FreeCAD CAx development system.              *
5
 *                                                                         *
6
 *   This library is free software; you can redistribute it and/or         *
7
 *   modify it under the terms of the GNU Library General Public           *
8
 *   License as published by the Free Software Foundation; either          *
9
 *   version 2 of the License, or (at your option) any later version.      *
10
 *                                                                         *
11
 *   This library  is distributed in the hope that it will be useful,      *
12
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
13
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
14
 *   GNU Library General Public License for more details.                  *
15
 *                                                                         *
16
 *   You should have received a copy of the GNU Library General Public     *
17
 *   License along with this library; see the file COPYING.LIB. If not,    *
18
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
19
 *   Suite 330, Boston, MA  02111-1307, USA                                *
20
 *                                                                         *
21
 ***************************************************************************/
22

23
#include "PreCompiled.h"
24

25
#include <cassert>
26
#include "DualQuaternion.h"
27

28

29
// NOLINTBEGIN(readability-identifier-length)
30
Base::DualQuat Base::operator+(Base::DualQuat a, Base::DualQuat b)
31
{
32
    return {a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w};
33
}
34

35
Base::DualQuat Base::operator-(Base::DualQuat a, Base::DualQuat b)
36
{
37
    return {a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w};
38
}
39

40
Base::DualQuat Base::operator*(Base::DualQuat a, Base::DualQuat b)
41
{
42
    return {a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
43
            a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
44
            a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
45
            a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z};
46
}
47

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

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

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

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

68
Base::DualQuat::DualQuat(Base::DualQuat re, Base::DualQuat du)
69
    : x(re.x.re, du.x.re)
70
    , y(re.y.re, du.y.re)
71
    , z(re.z.re, du.z.re)
72
    , w(re.w.re, du.w.re)
73
{
74
    assert(re.dual().length() < 1e-12);
75
    assert(du.dual().length() < 1e-12);
76
}
77

78
double Base::DualQuat::dot(Base::DualQuat a, Base::DualQuat b)
79
{
80
    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;
81
}
82

83
Base::DualQuat Base::DualQuat::pow(double t, bool shorten) const
84
{
85
    /* implemented after "Dual-Quaternions: From Classical Mechanics to
86
     * Computer Graphics and Beyond" by Ben Kenwright www.xbdev.net
87
     * bkenwright@xbdev.net
88
     * http://www.xbdev.net/misc_demos/demos/dual_quaternions_beyond/paper.pdf
89
     *
90
     * There are some differences:
91
     *
92
     * * Special handling of no-rotation situation (because normalization
93
     * multiplier becomes infinite in this situation, breaking the algorithm).
94
     *
95
     * * Dual quaternions are implemented as a collection of dual numbers,
96
     * rather than a collection of two quaternions like it is done in suggested
97
     * implementation in the paper.
98
     *
99
     * * acos replaced with atan2 for improved angle accuracy for small angles
100
     *
101
     * */
102
    double le = this->vec().length();
103
    if (le < 1e-12) {
104
        // special case of no rotation. Interpolate position
105
        return {this->real(), this->dual() * t};
106
    }
107

108
    double normmult = 1.0 / le;
109

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

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

126
    // interpolate
127
    theta *= t;
128
    pitch *= t;
129

130
    // back to quaternion
131
    return {l * sin(theta / 2) + DualQuat(0, 0, 0, cos(theta / 2)),
132
            m * sin(theta / 2) + pitch / 2 * cos(theta / 2) * l
133
                + DualQuat(0, 0, 0, -pitch / 2 * sin(theta / 2))};
134
}
135
// NOLINTEND(readability-identifier-length)
136