grey/BOSL2
1
0
Fork 0
mirror of https://github.com/BelfrySCAD/BOSL2.git synced 2025-01-30 00:09:37 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Changes in Q_Angle computation and formatting

Browse source
This commit is contained in:
RonaldoCMP 2020-07-19 22:09:33 +01:00
parent fd0e74ac97
commit 8129176171
1 changed files with 153 additions and 156 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

309
quaternions.scad
View file

@ -47,11 +47,11 @@ function Q_is_quat(q) = is_vector(q,4) && ! approx(norm(q),0) ;
// ax = Vector of axis of rotation. // ax = Vector of axis of rotation.
// ang = Number of degrees to rotate around the axis counter-clockwise, when facing the origin. // ang = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
function Quat(ax=[0,0,1], ang=0) = function Quat(ax=[0,0,1], ang=0) =
assert( is_vector(ax,3) && is_num(0*ang), "Invalid input") assert( is_vector(ax,3) && is_finite(ang), "Invalid input")
let( n = norm(ax) ) let( n = norm(ax) )
approx(n,0) approx(n,0)
? _Quat([0,0,0], sin(ang/2), cos(ang/2)) ? _Quat([0,0,0], sin(ang/2), cos(ang/2))
: _Quat(ax/n, sin(ang/2), cos(ang/2)); : _Quat(ax/n, sin(ang/2), cos(ang/2));
// Function: QuatX() // Function: QuatX()
@ -61,8 +61,8 @@ function Quat(ax=[0,0,1], ang=0) =
// Arguments: // Arguments:
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin. // a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
function QuatX(a=0) = function QuatX(a=0) =
assert( is_num(0*a), "Invalid angle" ) assert( is_finite(a), "Invalid angle" )
Quat([1,0,0],a); Quat([1,0,0],a);
// Function: QuatY() // Function: QuatY()
@ -72,8 +72,8 @@ function QuatX(a=0) =
// Arguments: // Arguments:
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin. // a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
function QuatY(a=0) = function QuatY(a=0) =
assert( is_num(0*a), "Invalid angle" ) assert( is_finite(a), "Invalid angle" )
Quat([0,1,0],a); Quat([0,1,0],a);
// Function: QuatZ() // Function: QuatZ()
@ -83,8 +83,8 @@ function QuatY(a=0) =
// Arguments: // Arguments:
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin. // a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
function QuatZ(a=0) = function QuatZ(a=0) =
assert( is_num(0*a), "Invalid angle" ) assert( is_finite(a), "Invalid angle" )
Quat([0,0,1],a); Quat([0,0,1],a);
// Function: QuatXYZ() // Function: QuatXYZ()
@ -95,11 +95,11 @@ function QuatZ(a=0) =
// Arguments: // Arguments:
// a = The triplet of rotation angles, [X,Y,Z] // a = The triplet of rotation angles, [X,Y,Z]
function QuatXYZ(a=[0,0,0]) = function QuatXYZ(a=[0,0,0]) =
assert( is_vector(a,3), "Invalid angles") assert( is_vector(a,3), "Invalid angles")
let( let(
qx = QuatX(a[0]), qx = QuatX(a[0]),
qy = QuatY(a[1]), qy = QuatY(a[1]),
qz = QuatZ(a[2]) qz = QuatZ(a[2])
) )
Q_Mul(qz, Q_Mul(qy, qx)); Q_Mul(qz, Q_Mul(qy, qx));
@ -111,14 +111,14 @@ function QuatXYZ(a=[0,0,0]) =
// Returns the normalized quaternion that rotates the non zero 3D vector v1 // Returns the normalized quaternion that rotates the non zero 3D vector v1
// to the non zero 3D vector v2. // to the non zero 3D vector v2.
function Q_From_to(v1, v2) = function Q_From_to(v1, v2) =
assert( is_vector(v1,3) && is_vector(v2,3) assert( is_vector(v1,3) && is_vector(v2,3)
&& ! approx(norm(v1),0) && ! approx(norm(v2),0) && ! approx(norm(v1),0) && ! approx(norm(v2),0)
, "Invalid vector(s)") , "Invalid vector(s)")
let( ax = cross(v1,v2), let( ax = cross(v1,v2),
n = norm(ax) ) n = norm(ax) )
approx(n, 0) approx(n, 0)
? v1*v2>0 ? Q_Ident() : Quat([ v1.y, -v1.x, 0], 180) ? v1*v2>0 ? Q_Ident() : Quat([ v1.y, -v1.x, 0], 180)
: Quat(ax, atan2( n , v1*v2 )); : Quat(ax, atan2( n , v1*v2 ));
// Function: Q_Ident() // Function: Q_Ident()
@ -133,8 +133,8 @@ function Q_Ident() = [0, 0, 0, 1];
// Adds a scalar value `s` to the W part of a quaternion `q`. // Adds a scalar value `s` to the W part of a quaternion `q`.
// The returned quaternion is usually not normalized. // The returned quaternion is usually not normalized.
function Q_Add_S(q, s) = function Q_Add_S(q, s) =
assert( is_num(0*s), "Invalid scalar" ) assert( is_finite(s), "Invalid scalar" )
q+[0,0,0,s]; q+[0,0,0,s];
// Function: Q_Sub_S() // Function: Q_Sub_S()
@ -144,8 +144,8 @@ function Q_Add_S(q, s) =
// Subtracts a scalar value `s` from the W part of a quaternion `q`. // Subtracts a scalar value `s` from the W part of a quaternion `q`.
// The returned quaternion is usually not normalized. // The returned quaternion is usually not normalized.
function Q_Sub_S(q, s) = function Q_Sub_S(q, s) =
assert( is_num(0*s), "Invalid scalar" ) assert( is_finite(s), "Invalid scalar" )
q-[0,0,0,s]; q-[0,0,0,s];
// Function: Q_Mul_S() // Function: Q_Mul_S()
@ -155,8 +155,8 @@ function Q_Sub_S(q, s) =
// Multiplies each part of a quaternion `q` by a scalar value `s`. // Multiplies each part of a quaternion `q` by a scalar value `s`.
// The returned quaternion is usually not normalized. // The returned quaternion is usually not normalized.
function Q_Mul_S(q, s) = function Q_Mul_S(q, s) =
assert( is_num(0*s), "Invalid scalar" ) assert( is_finite(s), "Invalid scalar" )
q*s; q*s;
// Function: Q_Div_S() // Function: Q_Div_S()
@ -166,8 +166,8 @@ function Q_Mul_S(q, s) =
// Divides each part of a quaternion `q` by a scalar value `s`. // Divides each part of a quaternion `q` by a scalar value `s`.
// The returned quaternion is usually not normalized. // The returned quaternion is usually not normalized.
function Q_Div_S(q, s) = function Q_Div_S(q, s) =
assert( is_num(0*s) && ! approx(s,0) , "Invalid scalar" ) assert( is_finite(s) && ! approx(s,0) , "Invalid scalar" )
q/s; q/s;
// Function: Q_Add() // Function: Q_Add()
@ -177,9 +177,9 @@ function Q_Div_S(q, s) =
// Adds each part of two quaternions together. // Adds each part of two quaternions together.
// The returned quaternion is usually not normalized. // The returned quaternion is usually not normalized.
function Q_Add(a, b) = function Q_Add(a, b) =
assert( Q_is_quat(a) && Q_is_quat(a), "Invalid quaternion(s)") assert( Q_is_quat(a) && Q_is_quat(a), "Invalid quaternion(s)")
assert( ! approx(norm(a+b),0), "Quaternions cannot be opposed" ) assert( ! approx(norm(a+b),0), "Quaternions cannot be opposed" )
a+b; a+b;
// Function: Q_Sub() // Function: Q_Sub()
@ -189,9 +189,9 @@ function Q_Add(a, b) =
// Subtracts each part of quaternion `b` from quaternion `a`. // Subtracts each part of quaternion `b` from quaternion `a`.
// The returned quaternion is usually not normalized. // The returned quaternion is usually not normalized.
function Q_Sub(a, b) = function Q_Sub(a, b) =
assert( Q_is_quat(a) && Q_is_quat(a), "Invalid quaternion(s)") assert( Q_is_quat(a) && Q_is_quat(a), "Invalid quaternion(s)")
assert( ! approx(a,b), "Quaternions cannot be equal" ) assert( ! approx(a,b), "Quaternions cannot be equal" )
a-b; a-b;
// Function: Q_Mul() // Function: Q_Mul()
@ -201,13 +201,13 @@ function Q_Sub(a, b) =
// Multiplies quaternion `a` by quaternion `b`. // Multiplies quaternion `a` by quaternion `b`.
// The returned quaternion is normalized if both `a` and `b` are normalized // The returned quaternion is normalized if both `a` and `b` are normalized
function Q_Mul(a, b) = function Q_Mul(a, b) =
assert( Q_is_quat(a) && Q_is_quat(b), "Invalid quaternion(s)") assert( Q_is_quat(a) && Q_is_quat(b), "Invalid quaternion(s)")
[ [
a[3]*b.x + a.x*b[3] + a.y*b.z - a.z*b.y, a[3]*b.x + a.x*b[3] + a.y*b.z - a.z*b.y,
a[3]*b.y - a.x*b.z + a.y*b[3] + a.z*b.x, a[3]*b.y - a.x*b.z + a.y*b[3] + a.z*b.x,
a[3]*b.z + a.x*b.y - a.y*b.x + a.z*b[3], a[3]*b.z + a.x*b.y - a.y*b.x + a.z*b[3],
a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z, a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z,
]; ];
// Function: Q_Cumulative() // Function: Q_Cumulative()
@ -235,16 +235,16 @@ function Q_Cumulative(v, _i=0, _acc=[]) =
// Q_Dot(a, b) // Q_Dot(a, b)
// Description: Calculates the dot product between quaternions `a` and `b`. // Description: Calculates the dot product between quaternions `a` and `b`.
function Q_Dot(a, b) = function Q_Dot(a, b) =
assert( Q_is_quat(a) && Q_is_quat(b), "Invalid quaternion(s)" ) assert( Q_is_quat(a) && Q_is_quat(b), "Invalid quaternion(s)" )
a*b; a*b;
// Function: Q_Neg() // Function: Q_Neg()
// Usage: // Usage:
// Q_Neg(q) // Q_Neg(q)
// Description: Returns the negative of quaternion `q`. // Description: Returns the negative of quaternion `q`.
function Q_Neg(q) = function Q_Neg(q) =
assert( Q_is_quat(q), "Invalid quaternion" ) assert( Q_is_quat(q), "Invalid quaternion" )
-q; -q;
// Function: Q_Conj() // Function: Q_Conj()
@ -252,8 +252,8 @@ function Q_Neg(q) =
// Q_Conj(q) // Q_Conj(q)
// Description: Returns the conjugate of quaternion `q`. // Description: Returns the conjugate of quaternion `q`.
function Q_Conj(q) = function Q_Conj(q) =
assert( Q_is_quat(q), "Invalid quaternion" ) assert( Q_is_quat(q), "Invalid quaternion" )
[-q.x, -q.y, -q.z, q[3]]; [-q.x, -q.y, -q.z, q[3]];
// Function: Q_Inverse() // Function: Q_Inverse()
@ -261,9 +261,9 @@ function Q_Conj(q) =
// qc = Q_Inverse(q) // qc = Q_Inverse(q)
// Description: Returns the multiplication inverse of quaternion `q` that is normalized only if `q` is normalized. // Description: Returns the multiplication inverse of quaternion `q` that is normalized only if `q` is normalized.
function Q_Inverse(q) = function Q_Inverse(q) =
assert( Q_is_quat(q), "Invalid quaternion" ) assert( Q_is_quat(q), "Invalid quaternion" )
let(q = _Qnorm(q) ) let(q = _Qnorm(q) )
[-q.x, -q.y, -q.z, q[3]]; [-q.x, -q.y, -q.z, q[3]];
// Function: Q_Norm() // Function: Q_Norm()
@ -273,8 +273,8 @@ function Q_Inverse(q) =
// Returns the `norm()` "length" of quaternion `q`. // Returns the `norm()` "length" of quaternion `q`.
// Normalized quaternions have unitary norm. // Normalized quaternions have unitary norm.
function Q_Norm(q) = function Q_Norm(q) =
assert( Q_is_quat(q), "Invalid quaternion" ) assert( Q_is_quat(q), "Invalid quaternion" )
norm(q); norm(q);
// Function: Q_Normalize() // Function: Q_Normalize()
@ -282,8 +282,8 @@ function Q_Norm(q) =
// Q_Normalize(q) // Q_Normalize(q)
// Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1. // Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1.
function Q_Normalize(q) = function Q_Normalize(q) =
assert( Q_is_quat(q) , "Invalid quaternion" ) assert( Q_is_quat(q) , "Invalid quaternion" )
q/norm(q); q/norm(q);
// Function: Q_Dist() // Function: Q_Dist()
@ -291,8 +291,8 @@ function Q_Normalize(q) =
// Q_Dist(q1, q2) // Q_Dist(q1, q2)
// Description: Returns the "distance" between two quaternions. // Description: Returns the "distance" between two quaternions.
function Q_Dist(q1, q2) = function Q_Dist(q1, q2) =
assert( Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) assert( Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" )
norm(q2-q1); norm(q2-q1);
// Function: Q_Slerp() // Function: Q_Slerp()
@ -318,25 +318,23 @@ function Q_Dist(q1, q2) =
// Qrot(q) right(80) cube([10,10,1]); // Qrot(q) right(80) cube([10,10,1]);
// #sphere(r=80); // #sphere(r=80);
function Q_Slerp(q1, q2, u, _dot) = function Q_Slerp(q1, q2, u, _dot) =
is_undef(_dot) is_undef(_dot)
? assert(is_num(u) || is_range(u) || is_num(0*u*u), "Invalid interpolation coefficient(s)") ? assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(s)")
assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" )
let( let(
_dot = q1*q2, _dot = q1*q2,
q1 = q1/norm(q1), q1 = q1/norm(q1),
q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2),
dot = abs(_dot) dot = abs(_dot)
) )
! is_num(u) ! is_finite(u) ? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] :
? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] Q_Slerp(q1, q2, u, dot)
: Q_Slerp(q1, q2, u, dot) : _dot>0.9995
: _dot>0.9995 ? _Qnorm(q1 + u*(q2-q1))
? _Qnorm(q1 + u*(q2-q1)) : let( theta = u*acos(_dot),
: let( q3 = _Qnorm(q2 - _dot*q1)
theta = u*acos(_dot), )
q3 = _Qnorm(q2 - _dot*q1) _Qnorm(q1*cos(theta) + q3*sin(theta));
)
_Qnorm(q1*cos(theta) + q3*sin(theta));
// Function: Q_Matrix3() // Function: Q_Matrix3()
@ -345,12 +343,12 @@ function Q_Slerp(q1, q2, u, _dot) =
// Description: // Description:
// Returns the 3x3 rotation matrix for the given normalized quaternion q. // Returns the 3x3 rotation matrix for the given normalized quaternion q.
function Q_Matrix3(q) = function Q_Matrix3(q) =
let( q = Q_Normalize(q) ) let( q = Q_Normalize(q) )
[ [
[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]], [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]],
[ 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]], [ 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]],
[ 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]] [ 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]]
]; ];
// Function: Q_Matrix4() // Function: Q_Matrix4()
@ -359,13 +357,13 @@ function Q_Matrix3(q) =
// Description: // Description:
// Returns the 4x4 rotation matrix for the given normalized quaternion q. // Returns the 4x4 rotation matrix for the given normalized quaternion q.
function Q_Matrix4(q) = function Q_Matrix4(q) =
let( q = Q_Normalize(q) ) let( q = Q_Normalize(q) )
[ [
[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], [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],
[ 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], [ 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],
[ 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], [ 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],
[ 0, 0, 0, 1] [ 0, 0, 0, 1]
]; ];
// Function: Q_Axis() // Function: Q_Axis()
@ -375,9 +373,9 @@ function Q_Matrix4(q) =
// Returns the axis of rotation of a normalized quaternion `q`. // Returns the axis of rotation of a normalized quaternion `q`.
// The input doesn't need to be normalized. // The input doesn't need to be normalized.
function Q_Axis(q) = function Q_Axis(q) =
assert( Q_is_quat(q) , "Invalid quaternion" ) assert( Q_is_quat(q) , "Invalid quaternion" )
let( d = norm(_Qvec(q)) ) let( d = norm(_Qvec(q)) )
approx(d,0)? [0,0,1] : _Qvec(q)/d; approx(d,0)? [0,0,1] : _Qvec(q)/d;
// Function: Q_Angle() // Function: Q_Angle()
// Usage: // Usage:
@ -388,12 +386,13 @@ function Q_Axis(q) =
// If both q1 and q2 are given, returns the angle (in degrees) between them. // If both q1 and q2 are given, returns the angle (in degrees) between them.
// The input quaternions don't need to be normalized. // The input quaternions don't need to be normalized.
function Q_Angle(q1,q2) = function Q_Angle(q1,q2) =
assert(Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2)), "Invalid quaternion(s)" ) assert(Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2)), "Invalid quaternion(s)" )
let( n1 = is_undef(q2)? norm(_Qvec(q1)): norm(q1) ) let( n1 = is_undef(q2)? norm(_Qvec(q1)): norm(q1) )
is_undef(q2) is_undef(q2)
? 2 * atan2(n1,_Qreal(q1)) ? 2 * atan2(n1,_Qreal(q1))
: 2 * acos(q1*q2/n1/norm(q2)) ; : let( q1 = q1/norm(q1),
q2 = q2/norm(q2) )
4 * atan2(norm(q1 - q2), norm(q1 + q2));
// Function&Module: Qrot() // Function&Module: Qrot()
// Usage: As Module // Usage: As Module
@ -425,9 +424,9 @@ module Qrot(q) {
} }
function Qrot(q,p) = function Qrot(q,p) =
is_undef(p)? Q_Matrix4(q) : is_undef(p)? Q_Matrix4(q) :
is_vector(p)? Qrot(q,[p])[0] : is_vector(p)? Qrot(q,[p])[0] :
apply(Q_Matrix4(q), p); apply(Q_Matrix4(q), p);
// Module: Qrot_copies() // Module: Qrot_copies()
@ -457,24 +456,24 @@ module Qrot_copies(quats) for (q=quats) Qrot(q) children();
// It doesn't check whether R is in fact a rotation matrix. // It doesn't check whether R is in fact a rotation matrix.
// If R is not a rotation, the returned quaternion is an unpredictable quaternion . // If R is not a rotation, the returned quaternion is an unpredictable quaternion .
function Q_Rotation(R) = function Q_Rotation(R) =
assert( is_matrix(R,3,3) || is_matrix(R,4,4) , assert( is_matrix(R,3,3) || is_matrix(R,4,4) ,
"Matrix is neither 3x3 nor 4x4") "Matrix is neither 3x3 nor 4x4")
let( tr = R[0][0]+R[1][1]+R[2][2] ) // R trace let( tr = R[0][0]+R[1][1]+R[2][2] ) // R trace
tr>0 tr>0
? let( r = 1+tr ) ? let( r = 1+tr )
_Qnorm( _Qset([ R[1][2]-R[2][1], R[2][0]-R[0][2], R[0][1]-R[1][0] ], -r ) ) _Qnorm( _Qset([ R[1][2]-R[2][1], R[2][0]-R[0][2], R[0][1]-R[1][0] ], -r ) )
: let( i = max_index([ R[0][0], R[1][1], R[2][2] ]), : let( i = max_index([ R[0][0], R[1][1], R[2][2] ]),
r = 1 + 2*R[i][i] -R[0][0] -R[1][1] -R[2][2] ) r = 1 + 2*R[i][i] -R[0][0] -R[1][1] -R[2][2] )
i==0 ? _Qnorm( _Qset( [ 4*r, (R[1][0]+R[0][1]), (R[0][2]+R[2][0]) ], (R[2][1]-R[1][2])) ): i==0 ? _Qnorm( _Qset( [ 4*r, (R[1][0]+R[0][1]), (R[0][2]+R[2][0]) ], (R[2][1]-R[1][2])) ):
i==1 ? _Qnorm( _Qset( [ (R[1][0]+R[0][1]), 4*r, (R[2][1]+R[1][2]) ], (R[0][2]-R[2][0])) ) i==1 ? _Qnorm( _Qset( [ (R[1][0]+R[0][1]), 4*r, (R[2][1]+R[1][2]) ], (R[0][2]-R[2][0])) ):
: _Qnorm( _Qset( [ (R[2][0]+R[0][2]), (R[1][2]+R[2][1]), 4*r ], (R[1][0]-R[0][1])) ); _Qnorm( _Qset( [ (R[2][0]+R[0][2]), (R[1][2]+R[2][1]), 4*r ], (R[1][0]-R[0][1])) ) ;
// Function&Module: Q_Rotation_path(q1, n, [q2]) // Function&Module: Q_Rotation_path(q1, n, [q2])
// Usage as a function: // Usage: As a function
// path = Q_Rotation_path(q1, n, q2); // path = Q_Rotation_path(q1, n, q2);
// path = Q_Rotation_path(q1, n); // path = Q_Rotation_path(q1, n);
// Usage as a module: // Usage: As a module
// Q_Rotation_path(q1, n, q2) ... // Q_Rotation_path(q1, n, q2) ...
// Description: // Description:
// If q2 is undef and it is called as a function, the path, with length n+1 (n>=1), will be the // If q2 is undef and it is called as a function, the path, with length n+1 (n>=1), will be the
@ -515,20 +514,20 @@ function Q_Rotation(R) =
// right(80) cube([10,10,1]); // right(80) cube([10,10,1]);
// #sphere(r=80); // #sphere(r=80);
function Q_Rotation_path(q1, n=1, q2) = function Q_Rotation_path(q1, n=1, q2) =
assert( Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2) ), "Invalid quaternion(s)" ) assert( Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2) ), "Invalid quaternion(s)" )
assert( is_num(0*n) && n>=1 && n==floor(n), "Invalid integer" ) assert( is_finite(n) && n>=1 && n==floor(n), "Invalid integer" )
assert( is_undef(q2) || ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" ) assert( is_undef(q2) || ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" )
is_undef(q2) is_undef(q2)
? [for( i=0, dR=Q_Matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R] ? [for( i=0, dR=Q_Matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R]
: let( q2 = Q_Normalize( q1*q2<0 ? -q2: q2 ) ) : let( q2 = Q_Normalize( q1*q2<0 ? -q2: q2 ),
let( dq = Q_pow( Q_Mul( q2, Q_Inverse(q1) ), 1/n ), dq = Q_pow( Q_Mul( q2, Q_Inverse(q1) ), 1/n ),
dR = Q_Matrix4(dq) ) dR = Q_Matrix4(dq) )
[for( i=0, R=Q_Matrix4(q1); i<=n; i=i+1, R=dR*R ) R]; [for( i=0, R=Q_Matrix4(q1); i<=n; i=i+1, R=dR*R ) R];
module Q_Rotation_path(q1, n=1, q2) { module Q_Rotation_path(q1, n=1, q2) {
for(Mi=Q_Rotation_path(q1, n, q2)) for(Mi=Q_Rotation_path(q1, n, q2))
multmatrix(Mi) multmatrix(Mi)
children(); children();
} }
@ -559,15 +558,15 @@ module Q_Rotation_path(q1, n=1, q2) {
// Qrot(q) right(80) cube([10,10,1]); // Qrot(q) right(80) cube([10,10,1]);
// #sphere(r=80); // #sphere(r=80);
function Q_Nlerp(q1,q2,u) = function Q_Nlerp(q1,q2,u) =
assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) assert(is_finite(u) || is_range(u) || is_vector(u) ,
assert( ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" ) "Invalid interpolation coefficient(s)" )
assert(is_num(0*u) || is_range(u) || (is_list(u) && is_num(0*u*u)) , assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" )
"Invalid interpolation coefficient(s)" ) assert( ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" )
let( q1 = Q_Normalize(q1), let( q1 = Q_Normalize(q1),
q2 = Q_Normalize(q2) ) q2 = Q_Normalize(q2) )
!is_num(u) is_num(u)
? [for (ui=u) _Qnorm((1-ui)*q1 + ui*q2 ) ] ? _Qnorm((1-u)*q1 + u*q2 )
: _Qnorm((1-u)*q1 + u*q2 ); : [for (ui=u) _Qnorm((1-ui)*q1 + ui*q2 ) ];
// Function: Q_Squad() // Function: Q_Squad()
@ -610,11 +609,11 @@ function Q_Nlerp(q1,q2,u) =
// Qrot(q) right(80) cube([10,10,1]); // Qrot(q) right(80) cube([10,10,1]);
// #sphere(r=80); // #sphere(r=80);
function Q_Squad(q1,q2,q3,q4,u) = function Q_Squad(q1,q2,q3,q4,u) =
assert(is_num(0*u) || is_range(u) || (is_list(u) && is_num(0*u*u)) , assert(is_finite(u) || is_range(u) || is_vector(u) ,
"Invalid interpolation coefficient(s)" ) "Invalid interpolation coefficient(s)" )
is_num(u) is_num(u)
? Q_Slerp( Q_Slerp(q1,q4,u), Q_Slerp(q2,q3,u), 2*u*(1-u)) ? Q_Slerp( Q_Slerp(q1,q4,u), Q_Slerp(q2,q3,u), 2*u*(1-u))
: [for(ui=u) Q_Slerp( Q_Slerp(q1,q4,ui), Q_Slerp(q2,q3,ui), 2*ui*(1-ui) ) ]; : [for(ui=u) Q_Slerp( Q_Slerp(q1,q4,ui), Q_Slerp(q2,q3,ui), 2*ui*(1-ui) ) ];
// Function: Q_exp() // Function: Q_exp()
@ -624,9 +623,9 @@ function Q_Squad(q1,q2,q3,q4,u) =
// Returns the quaternion that is the exponential of the quaternion q in base e // Returns the quaternion that is the exponential of the quaternion q in base e
// The returned quaternion is usually not normalized. // The returned quaternion is usually not normalized.
function Q_exp(q) = function Q_exp(q) =
assert( is_vector(q,4), "Input is not a valid quaternion") assert( is_vector(q,4), "Input is not a valid quaternion")
let( nv = norm(_Qvec(q)) ) // q may be equal to zero! let( nv = norm(_Qvec(q)) ) // q may be equal to zero here!
exp(_Qreal(q))*Quat(_Qvec(q),2*nv); exp(_Qreal(q))*Quat(_Qvec(q),2*nv);
// Function: Q_ln() // Function: Q_ln()
@ -636,12 +635,11 @@ function Q_exp(q) =
// Returns the quaternion that is the natural logarithm of the quaternion q. // Returns the quaternion that is the natural logarithm of the quaternion q.
// The returned quaternion is usually not normalized and may be zero. // The returned quaternion is usually not normalized and may be zero.
function Q_ln(q) = function Q_ln(q) =
assert(Q_is_quat(q), "Input is not a valid quaternion") assert(Q_is_quat(q), "Input is not a valid quaternion")
let( nq = norm(q), let( nq = norm(q),
nv = norm(_Qvec(q)) ) nv = norm(_Qvec(q)) )
approx(nv,0) approx(nv,0) ? _Qset([0,0,0] , ln(nq) ) :
? _Qset([0,0,0] , ln(nq) ) _Qset(_Qvec(q)*atan2(nv,_Qreal(q))/nv, ln(nq));
: _Qset(_Qvec(q)*atan2(nv,_Qreal(q))/nv, ln(nq));
// Function: Q_pow() // Function: Q_pow()
@ -651,11 +649,10 @@ function Q_ln(q) =
// Returns the quaternion that is the power of the quaternion q to the real exponent r. // Returns the quaternion that is the power of the quaternion q to the real exponent r.
// The returned quaternion is normalized if `q` is normalized. // The returned quaternion is normalized if `q` is normalized.
function Q_pow(q,r=1) = function Q_pow(q,r=1) =
// Q_exp(r*Q_ln(q)); assert( Q_is_quat(q) && is_finite(r),
assert( Q_is_quat(q) && is_num(0*r), "Invalid inputs")
"Invalid inputs") let( theta = 2*atan2(norm(_Qvec(q)),_Qreal(q)) )
let( theta = 2*atan2(norm(_Qvec(q)),_Qreal(q)) ) Quat(_Qvec(q), r*theta); // Q_exp(r*Q_ln(q));
Quat(_Qvec(q), r*theta);