1///////////////////////////////////////////
2// LibFile: quaternions.scad
3// Support for Quaternions.
4// To use, add the following line to the beginning of your file:
5// ```
6// use <BOSL/quaternions.scad>
7// ```
8///////////////////////////////////////////
9
10/*
11BSD 2-Clause License
12
13Copyright (c) 2017, Revar Desmera
14All rights reserved.
15
16Redistribution and use in source and binary forms, with or without
17modification, are permitted provided that the following conditions are met:
18
19* Redistributions of source code must retain the above copyright notice, this
20 list of conditions and the following disclaimer.
21
22* Redistributions in binary form must reproduce the above copyright notice,
23 this list of conditions and the following disclaimer in the documentation
24 and/or other materials provided with the distribution.
25
26THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
27AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
28IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
29DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
30FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
31DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
32SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
34OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
35OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36*/
37
38
39use <math.scad>
40
41
42// Section: Quaternions
43// Quaternions are fast methods of storing and calculating arbitrary rotations.
44// Quaternions contain information on both axis of rotation, and rotation angle.
45// You can chain multiple rotation together by multiplying quaternions together.
46// They don't suffer from the gimbal-lock issues that [X,Y,Z] rotation angles do.
47// Quaternions are stored internally as a 4-value vector:
48// `[X, Y, Z, W] = W + Xi + Yj + Zk`
49
50
51// Internal
52function _Quat(a,s,w) = [a[0]*s, a[1]*s, a[2]*s, w];
53
54
55// Function: Quat()
56// Usage:
57// Quat(ax, ang);
58// Description: Create a new Quaternion from axis and angle of rotation.
59// Arguments:
60// ax = Vector of axis of rotation.
61// ang = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
62function Quat(ax=[0,0,1], ang=0) = _Quat(ax/norm(ax), sin(ang/2), cos(ang/2));
63
64
65// Function: QuatX()
66// Usage:
67// QuatX(a);
68// Description: Create a new Quaternion for rotating around the X axis [1,0,0].
69// Arguments:
70// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
71function QuatX(a=0) = Quat([1,0,0],a);
72
73
74// Function: QuatY()
75// Usage:
76// QuatY(a);
77// Description: Create a new Quaternion for rotating around the Y axis [0,1,0].
78// Arguments:
79// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
80function QuatY(a=0) = Quat([0,1,0],a);
81
82// Function: QuatZ()
83// Usage:
84// QuatZ(a);
85// Description: Create a new Quaternion for rotating around the Z axis [0,0,1].
86// Arguments:
87// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
88function QuatZ(a=0) = Quat([0,0,1],a);
89
90
91// Function: QuatXYZ()
92// Usage:
93// QuatXYZ([X,Y,Z])
94// Description:
95// Creates a quaternion from standard [X,Y,Z] rotation angles in degrees.
96// Arguments:
97// a = The triplet of rotation angles, [X,Y,Z]
98function QuatXYZ(a=[0,0,0]) =
99 let(
100 qx = QuatX(a[0]),
101 qy = QuatY(a[1]),
102 qz = QuatZ(a[2])
103 )
104 Q_Mul(qz, Q_Mul(qy, qx));
105
106
107// Function: Q_Ident()
108// Description: Returns the "Identity" zero-rotation Quaternion.
109function Q_Ident() = [0, 0, 0, 1];
110
111
112// Function: Q_Add_S()
113// Usage:
114// Q_Add_S(q, s)
115// Description: Adds a scalar value `s` to the W part of a quaternion `q`.
116function Q_Add_S(q, s) = q+[0,0,0,s];
117
118
119// Function: Q_Sub_S()
120// Usage:
121// Q_Sub_S(q, s)
122// Description: Subtracts a scalar value `s` from the W part of a quaternion `q`.
123function Q_Sub_S(q, s) = q-[0,0,0,s];
124
125
126// Function: Q_Mul_S()
127// Usage:
128// Q_Mul_S(q, s)
129// Description: Multiplies each part of a quaternion `q` by a scalar value `s`.
130function Q_Mul_S(q, s) = q*s;
131
132
133// Function: Q_Div_S()
134// Usage:
135// Q_Div_S(q, s)
136// Description: Divides each part of a quaternion `q` by a scalar value `s`.
137function Q_Div_S(q, s) = q/s;
138
139
140// Function: Q_Add()
141// Usage:
142// Q_Add(a, b)
143// Description: Adds each part of two quaternions together.
144function Q_Add(a, b) = a+b;
145
146
147// Function: Q_Sub()
148// Usage:
149// Q_Sub(a, b)
150// Description: Subtracts each part of quaternion `b` from quaternion `a`.
151function Q_Sub(a, b) = a-b;
152
153
154// Function: Q_Mul()
155// Usage:
156// Q_Mul(a, b)
157// Description: Multiplies quaternion `a` by quaternion `b`.
158function Q_Mul(a, b) = [
159 a[3]*b.x + a.x*b[3] + a.y*b.z - a.z*b.y,
160 a[3]*b.y - a.x*b.z + a.y*b[3] + a.z*b.x,
161 a[3]*b.z + a.x*b.y - a.y*b.x + a.z*b[3],
162 a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z,
163];
164
165
166// Function: Q_Dot()
167// Usage:
168// Q_Dot(a, b)
169// Description: Calculates the dot product between quaternions `a` and `b`.
170function Q_Dot(a, b) = a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
171
172
173// Function: Q_Neg()
174// Usage:
175// Q_Neg(q)
176// Description: Returns the negative of quaternion `q`.
177function Q_Neg(q) = -q;
178
179
180// Function: Q_Conj()
181// Usage:
182// Q_Conj(q)
183// Description: Returns the conjugate of quaternion `q`.
184function Q_Conj(q) = [-q.x, -q.y, -q.z, q[3]];
185
186
187// Function: Q_Norm()
188// Usage:
189// Q_Norm(q)
190// Description: Returns the `norm()` "length" of quaternion `q`.
191function Q_Norm(q) = norm(q);
192
193
194// Function: Q_Normalize()
195// Usage:
196// Q_Normalize(q)
197// Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1.
198function Q_Normalize(q) = q/norm(q);
199
200
201// Function: Q_Dist()
202// Usage:
203// Q_Dist(q1, q2)
204// Description: Returns the "distance" between two quaternions.
205function Q_Dist(q1, q2) = norm(q2-q1);
206
207
208// Function: Q_Slerp()
209// Usage:
210// Q_Slerp(q1, q2, u);
211// Description:
212// Returns a quaternion that is a spherical interpolation between two quaternions.
213// Arguments:
214// q1 = The first quaternion. (u=0)
215// q2 = The second quaternion. (u=1)
216// u = The proportional value, from 0 to 1, of what part of the interpolation to return.
217// Example(3D):
218// a = QuatY(15);
219// b = QuatY(75);
220// color("blue",0.25) Qrot(a) cylinder(d=1, h=80);
221// color("red",0.25) Qrot(b) cylinder(d=1, h=80);
222// Qrot(Q_Slerp(a, b, 0.6)) cylinder(d=1, h=80);
223function Q_Slerp(q1, q2, u) = let(
224 dot = Q_Dot(q1, q2),
225 qq2 = dot<0? Q_Neg(q2) : q2,
226 dott = dot<0? -dot : dot,
227 theta = u * acos(constrain(dott,-1,1))
228 ) (dott>0.9995)?
229 Q_Normalize(q1 + ((qq2-q1) * u)) :
230 (q1*cos(theta) + (Q_Normalize(qq2 - (q1 * dott)) * sin(theta)));
231
232
233// Function: Q_Matrix3()
234// Usage:
235// Q_Matrix3(q);
236// Description:
237// Returns the 3x3 rotation matrix for the given normalized quaternion q.
238function Q_Matrix3(q) = [
239 [1-2*q[1]*q[1]-2*q[2]*q[2], 2*q[0]*q[1]-2*q[2]*q[3], 2*q[0]*q[2]+2*q[1]*q[3]],
240 [ 2*q[0]*q[1]+2*q[2]*q[3], 1-2*q[0]*q[0]-2*q[2]*q[2], 2*q[1]*q[2]-2*q[0]*q[3]],
241 [ 2*q[0]*q[2]-2*q[1]*q[3], 2*q[1]*q[2]+2*q[0]*q[3], 1-2*q[0]*q[0]-2*q[1]*q[1]]
242];
243
244
245// Function: Q_Matrix4()
246// Usage:
247// Q_Matrix4(q);
248// Description:
249// Returns the 4x4 rotation matrix for the given normalized quaternion q.
250function Q_Matrix4(q) = [
251 [1-2*q[1]*q[1]-2*q[2]*q[2], 2*q[0]*q[1]-2*q[2]*q[3], 2*q[0]*q[2]+2*q[1]*q[3], 0],
252 [ 2*q[0]*q[1]+2*q[2]*q[3], 1-2*q[0]*q[0]-2*q[2]*q[2], 2*q[1]*q[2]-2*q[0]*q[3], 0],
253 [ 2*q[0]*q[2]-2*q[1]*q[3], 2*q[1]*q[2]+2*q[0]*q[3], 1-2*q[0]*q[0]-2*q[1]*q[1], 0],
254 [ 0, 0, 0, 1]
255];
256
257
258// Function: Q_Axis()
259// Usage:
260// Q_Axis(q)
261// Description:
262// Returns the axis of rotation of a normalized quaternion `q`.
263function Q_Axis(q) = let(d = sqrt(1-(q[3]*q[3]))) (d==0)? [0,0,1] : [q[0]/d, q[1]/d, q[2]/d];
264
265
266// Function: Q_Angle()
267// Usage:
268// Q_Angle(q)
269// Description:
270// Returns the angle of rotation (in degrees) of a normalized quaternion `q`.
271function Q_Angle(q) = 2 * acos(q[3]);
272
273
274// Function: Q_Rot_Vector()
275// Usage:
276// Q_Rot_Vector(v,q);
277// Description:
278// Returns the vector `v` after rotating it by the quaternion `q`.
279function Q_Rot_Vector(v,q) = Q_Mul(Q_Mul(q,concat(v,0)),Q_Conj(q));
280
281
282// Module: Qrot()
283// Usage:
284// Qrot(q) ...
285// Description:
286// Rotate all children by the rotation stored in quaternion `q`.
287// Example(FlatSpin):
288// q = QuatXYZ([45,35,10]);
289// color("red",0.25) cylinder(d=1,h=80);
290// Qrot(q) cylinder(d=1,h=80);
291module Qrot(q) {
292 multmatrix(Q_Matrix4(q)) {
293 children();
294 }
295}
296
297
298// vim: noexpandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap