diff --git a/quaternions.scad b/quaternions.scad index 40712d9..e21801c 100644 --- a/quaternions.scad +++ b/quaternions.scad @@ -20,59 +20,107 @@ // Internal function _Quat(a,s,w) = [a[0]*s, a[1]*s, a[2]*s, w]; +function _Qvec(q) = [q.x,q.y,q.z]; + +function _Qreal(q) = q[3]; + +function _Qset(v,r) = concat( v, r ); + +// normalizes without checking +function _Qnorm(q) = q/norm(q); + + +// Function: Q_is_quat() +// Usage: +// if(Q_is_quat(q)) a=0; +// Description: Return true if q is a valid non-zero quaternion. +// Arguments: +// q = object to check. +function Q_is_quat(q) = is_vector(q,4) && ! approx(norm(q),0) ; + // Function: Quat() // Usage: // Quat(ax, ang); -// Description: Create a new Quaternion from axis and angle of rotation. +// Description: Create a normalized Quaternion from axis and angle of rotation. // Arguments: // ax = Vector of axis of rotation. // ang = Number of degrees to rotate around the axis counter-clockwise, when facing the origin. -function Quat(ax=[0,0,1], ang=0) = _Quat(ax/norm(ax), sin(ang/2), cos(ang/2)); +function Quat(ax=[0,0,1], ang=0) = + assert( is_vector(ax,3) && is_finite(ang), "Invalid input") + let( n = norm(ax) ) + approx(n,0) + ? _Quat([0,0,0], sin(ang/2), cos(ang/2)) + : _Quat(ax/n, sin(ang/2), cos(ang/2)); // Function: QuatX() // Usage: // QuatX(a); -// Description: Create a new Quaternion for rotating around the X axis [1,0,0]. +// Description: Create a normalized Quaternion for rotating around the X axis [1,0,0]. // Arguments: // a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin. -function QuatX(a=0) = Quat([1,0,0],a); +function QuatX(a=0) = + assert( is_finite(a), "Invalid angle" ) + Quat([1,0,0],a); // Function: QuatY() // Usage: // QuatY(a); -// Description: Create a new Quaternion for rotating around the Y axis [0,1,0]. +// Description: Create a normalized Quaternion for rotating around the Y axis [0,1,0]. // Arguments: // a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin. -function QuatY(a=0) = Quat([0,1,0],a); +function QuatY(a=0) = + assert( is_finite(a), "Invalid angle" ) + Quat([0,1,0],a); + // Function: QuatZ() // Usage: // QuatZ(a); -// Description: Create a new Quaternion for rotating around the Z axis [0,0,1]. +// Description: Create a normalized Quaternion for rotating around the Z axis [0,0,1]. // Arguments: // a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin. -function QuatZ(a=0) = Quat([0,0,1],a); +function QuatZ(a=0) = + assert( is_finite(a), "Invalid angle" ) + Quat([0,0,1],a); // Function: QuatXYZ() // Usage: // QuatXYZ([X,Y,Z]) // Description: -// Creates a quaternion from standard [X,Y,Z] rotation angles in degrees. +// Creates a normalized quaternion from standard [X,Y,Z] rotation angles in degrees. // Arguments: // a = The triplet of rotation angles, [X,Y,Z] function QuatXYZ(a=[0,0,0]) = + assert( is_vector(a,3), "Invalid angles") let( - qx = QuatX(a[0]), - qy = QuatY(a[1]), - qz = QuatZ(a[2]) + qx = QuatX(a[0]), + qy = QuatY(a[1]), + qz = QuatZ(a[2]) ) Q_Mul(qz, Q_Mul(qy, qx)); +// Function: Q_From_to() +// Usage: +// q = Q_From_to(v1, v2); +// Description: +// Returns the normalized quaternion that rotates the non zero 3D vector v1 +// to the non zero 3D vector v2. +function Q_From_to(v1, v2) = + assert( is_vector(v1,3) && is_vector(v2,3) + && ! approx(norm(v1),0) && ! approx(norm(v2),0) + , "Invalid vector(s)") + let( ax = cross(v1,v2), + n = norm(ax) ) + approx(n, 0) + ? v1*v2>0 ? Q_Ident() : Quat([ v1.y, -v1.x, 0], 180) + : Quat(ax, atan2( n , v1*v2 )); + + // Function: Q_Ident() // Description: Returns the "Identity" zero-rotation Quaternion. function Q_Ident() = [0, 0, 0, 1]; @@ -81,55 +129,85 @@ function Q_Ident() = [0, 0, 0, 1]; // Function: Q_Add_S() // Usage: // Q_Add_S(q, s) -// Description: Adds a scalar value `s` to the W part of a quaternion `q`. -function Q_Add_S(q, s) = q+[0,0,0,s]; +// Description: +// Adds a scalar value `s` to the W part of a quaternion `q`. +// The returned quaternion is usually not normalized. +function Q_Add_S(q, s) = + assert( is_finite(s), "Invalid scalar" ) + q+[0,0,0,s]; // Function: Q_Sub_S() // Usage: // Q_Sub_S(q, s) -// Description: Subtracts a scalar value `s` from the W part of a quaternion `q`. -function Q_Sub_S(q, s) = q-[0,0,0,s]; +// Description: +// Subtracts a scalar value `s` from the W part of a quaternion `q`. +// The returned quaternion is usually not normalized. +function Q_Sub_S(q, s) = + assert( is_finite(s), "Invalid scalar" ) + q-[0,0,0,s]; // Function: Q_Mul_S() // Usage: // Q_Mul_S(q, s) -// Description: Multiplies each part of a quaternion `q` by a scalar value `s`. -function Q_Mul_S(q, s) = q*s; +// Description: +// Multiplies each part of a quaternion `q` by a scalar value `s`. +// The returned quaternion is usually not normalized. +function Q_Mul_S(q, s) = + assert( is_finite(s), "Invalid scalar" ) + q*s; // Function: Q_Div_S() // Usage: // Q_Div_S(q, s) -// Description: Divides each part of a quaternion `q` by a scalar value `s`. -function Q_Div_S(q, s) = q/s; +// Description: +// Divides each part of a quaternion `q` by a scalar value `s`. +// The returned quaternion is usually not normalized. +function Q_Div_S(q, s) = + assert( is_finite(s) && ! approx(s,0) , "Invalid scalar" ) + q/s; // Function: Q_Add() // Usage: // Q_Add(a, b) -// Description: Adds each part of two quaternions together. -function Q_Add(a, b) = a+b; +// Description: +// Adds each part of two quaternions together. +// The returned quaternion is usually not normalized. +function Q_Add(a, b) = + assert( Q_is_quat(a) && Q_is_quat(a), "Invalid quaternion(s)") + assert( ! approx(norm(a+b),0), "Quaternions cannot be opposed" ) + a+b; // Function: Q_Sub() // Usage: // Q_Sub(a, b) -// Description: Subtracts each part of quaternion `b` from quaternion `a`. -function Q_Sub(a, b) = a-b; +// Description: +// Subtracts each part of quaternion `b` from quaternion `a`. +// The returned quaternion is usually not normalized. +function Q_Sub(a, b) = + assert( Q_is_quat(a) && Q_is_quat(a), "Invalid quaternion(s)") + assert( ! approx(a,b), "Quaternions cannot be equal" ) + a-b; // Function: Q_Mul() // Usage: // Q_Mul(a, b) -// Description: Multiplies quaternion `a` by quaternion `b`. -function Q_Mul(a, b) = [ - 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.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, -]; +// Description: +// Multiplies quaternion `a` by quaternion `b`. +// The returned quaternion is normalized if both `a` and `b` are normalized +function Q_Mul(a, b) = + 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.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[3] - a.x*b.x - a.y*b.y - a.z*b.z, + ]; // Function: Q_Cumulative() @@ -139,6 +217,8 @@ function Q_Mul(a, b) = [ // Given a list of Quaternions, cumulatively multiplies them, returning a list // of each cumulative Quaternion product. It starts with the first quaternion // given in the list, and applies successive quaternion rotations in list order. +// The quaternion in the returned list are normalized if each quaternion in v +// is normalized. function Q_Cumulative(v, _i=0, _acc=[]) = _i==len(v) ? _acc : Q_Cumulative( @@ -154,42 +234,65 @@ function Q_Cumulative(v, _i=0, _acc=[]) = // Usage: // Q_Dot(a, b) // Description: Calculates the dot product between quaternions `a` and `b`. -function Q_Dot(a, b) = a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]; - +function Q_Dot(a, b) = + assert( Q_is_quat(a) && Q_is_quat(b), "Invalid quaternion(s)" ) + a*b; // Function: Q_Neg() // Usage: // Q_Neg(q) // Description: Returns the negative of quaternion `q`. -function Q_Neg(q) = -q; +function Q_Neg(q) = + assert( Q_is_quat(q), "Invalid quaternion" ) + -q; // Function: Q_Conj() // Usage: // Q_Conj(q) // Description: Returns the conjugate of quaternion `q`. -function Q_Conj(q) = [-q.x, -q.y, -q.z, q[3]]; +function Q_Conj(q) = + assert( Q_is_quat(q), "Invalid quaternion" ) + [-q.x, -q.y, -q.z, q[3]]; + + +// Function: Q_Inverse() +// Usage: +// qc = Q_Inverse(q) +// Description: Returns the multiplication inverse of quaternion `q` that is normalized only if `q` is normalized. +function Q_Inverse(q) = + assert( Q_is_quat(q), "Invalid quaternion" ) + let(q = _Qnorm(q) ) + [-q.x, -q.y, -q.z, q[3]]; // Function: Q_Norm() // Usage: // Q_Norm(q) -// Description: Returns the `norm()` "length" of quaternion `q`. -function Q_Norm(q) = norm(q); +// Description: +// Returns the `norm()` "length" of quaternion `q`. +// Normalized quaternions have unitary norm. +function Q_Norm(q) = + assert( Q_is_quat(q), "Invalid quaternion" ) + norm(q); // Function: Q_Normalize() // Usage: // Q_Normalize(q) // Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1. -function Q_Normalize(q) = q/norm(q); +function Q_Normalize(q) = + assert( Q_is_quat(q) , "Invalid quaternion" ) + q/norm(q); // Function: Q_Dist() // Usage: // Q_Dist(q1, q2) // Description: Returns the "distance" between two quaternions. -function Q_Dist(q1, q2) = norm(q2-q1); +function Q_Dist(q1, q2) = + assert( Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) + norm(q2-q1); // Function: Q_Slerp() @@ -214,24 +317,24 @@ function Q_Dist(q1, q2) = norm(q2-q1); // for (q = Q_Slerp(a, b, [0:0.1:1])) // Qrot(q) right(80) cube([10,10,1]); // #sphere(r=80); -function Q_Slerp(q1, q2, u) = - assert(is_num(u) || is_num(u[0])) - !is_num(u)? [for (uu=u) Q_Slerp(q1,q2,uu)] : - let( - q1 = Q_Normalize(q1), - q2 = Q_Normalize(q2), - dot = Q_Dot(q1, q2) - ) let( - q2 = dot<0? Q_Neg(q2) : q2, - dot = dot<0? -dot : dot - ) (dot>0.9995)? Q_Normalize(q1 + (u * (q2-q1))) : - let( - dot = constrain(dot,-1,1), - theta_0 = acos(dot), - theta = theta_0 * u, - q3 = Q_Normalize(q2 - q1*dot), - out = q1*cos(theta) + q3*sin(theta) - ) out; +function Q_Slerp(q1, q2, u, _dot) = + is_undef(_dot) + ? 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)" ) + let( + _dot = q1*q2, + q1 = q1/norm(q1), + q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), + dot = abs(_dot) + ) + ! is_finite(u) ? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] : + Q_Slerp(q1, q2, u, dot) + : _dot>0.9995 + ? _Qnorm(q1 + u*(q2-q1)) + : let( theta = u*acos(_dot), + q3 = _Qnorm(q2 - _dot*q1) + ) + _Qnorm(q1*cos(theta) + q3*sin(theta)); // Function: Q_Matrix3() @@ -239,11 +342,13 @@ function Q_Slerp(q1, q2, u) = // Q_Matrix3(q); // Description: // Returns the 3x3 rotation matrix for the given normalized quaternion q. -function Q_Matrix3(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]], - [ 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]] -]; +function Q_Matrix3(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]], + [ 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]] + ]; // Function: Q_Matrix4() @@ -251,12 +356,14 @@ function Q_Matrix3(q) = [ // Q_Matrix4(q); // Description: // Returns the 4x4 rotation matrix for the given normalized quaternion q. -function Q_Matrix4(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], - [ 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], - [ 0, 0, 0, 1] -]; +function Q_Matrix4(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], + [ 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], + [ 0, 0, 0, 1] + ]; // Function: Q_Axis() @@ -264,16 +371,28 @@ function Q_Matrix4(q) = [ // Q_Axis(q) // Description: // Returns the axis of rotation of a normalized quaternion `q`. -function 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]; - +// The input doesn't need to be normalized. +function Q_Axis(q) = + assert( Q_is_quat(q) , "Invalid quaternion" ) + let( d = norm(_Qvec(q)) ) + approx(d,0)? [0,0,1] : _Qvec(q)/d; // Function: Q_Angle() // Usage: -// Q_Angle(q) +// a = Q_Angle(q) +// a12 = Q_Angle(q1,q2); // Description: -// Returns the angle of rotation (in degrees) of a normalized quaternion `q`. -function Q_Angle(q) = 2 * acos(q[3]); - +// If only q1 is given, returns the angle of rotation (in degrees) of that quaternion. +// If both q1 and q2 are given, returns the angle (in degrees) between them. +// The input quaternions don't need to be normalized. +function Q_Angle(q1,q2) = + 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) ) + is_undef(q2) + ? 2 * atan2(n1,_Qreal(q1)) + : let( q1 = q1/norm(q1), + q2 = q2/norm(q2) ) + 4 * atan2(norm(q1 - q2), norm(q1 + q2)); // Function&Module: Qrot() // Usage: As Module @@ -305,9 +424,9 @@ module Qrot(q) { } function Qrot(q,p) = - is_undef(p)? Q_Matrix4(q) : - is_vector(p)? Qrot(q,[p])[0] : - apply(Q_Matrix4(q), p); + is_undef(p)? Q_Matrix4(q) : + is_vector(p)? Qrot(q,[p])[0] : + apply(Q_Matrix4(q), p); // Module: Qrot_copies() @@ -327,4 +446,214 @@ function Qrot(q,p) = module Qrot_copies(quats) for (q=quats) Qrot(q) children(); +// Function: Q_Rotation() +// Usage: +// Q_Rotation(R) +// Description: +// Returns a normalized quaternion corresponding to the rotation matrix R. +// R may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. +// The last row and last column of R are ignored for 4x4 matrices. +// 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 . +function Q_Rotation(R) = + assert( is_matrix(R,3,3) || is_matrix(R,4,4) , + "Matrix is neither 3x3 nor 4x4") + let( tr = R[0][0]+R[1][1]+R[2][2] ) // R trace + tr>0 + ? 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 ) ) + : 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] ) + 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])) ): + _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]) +// Usage: As a function +// path = Q_Rotation_path(q1, n, q2); +// path = Q_Rotation_path(q1, n); +// Usage: As a module +// Q_Rotation_path(q1, n, q2) ... +// Description: +// If q2 is undef and it is called as a function, the path, with length n+1 (n>=1), will be the +// cumulative multiplications of the matrix rotation of q1 by itself. +// If q2 is defined and it is called as a function, returns a rotation matrix path of length n+1 (n>=1) +// that interpolates two given rotation quaternions. The first matrix of the sequence is the +// matrix rotation of q1 and the last one, the matrix rotation of q2. The intermediary matrix +// rotations are an uniform interpolation of the path extreme matrices. +// When called as a module, applies to its children() each rotation of the sequence computed +// by the function. +// The input quaternions don't need to be normalized. +// Arguments: +// q1 = The quaternion of the first rotation. +// q2 = The quaternion of the last rotation. +// n = An integer defining the path length ( path length = n+1). +// Example(3D): as a function +// a = QuatY(-135); +// b = QuatXYZ([0,-30,30]); +// for (M=Q_Rotation_path(a, 10, b)) +// multmatrix(M) +// right(80) cube([10,10,1]); +// #sphere(r=80); +// Example(3D): as a module +// a = QuatY(-135); +// b = QuatXYZ([0,-30,30]); +// Q_Rotation_path(a, 10, b) +// right(80) cube([10,10,1]); +// #sphere(r=80); +// Example(3D): as a function +// a = QuatY(5); +// for (M=Q_Rotation_path(a, 10)) +// multmatrix(M) +// right(80) cube([10,10,1]); +// #sphere(r=80); +// Example(3D): as a module +// a = QuatY(5); +// Q_Rotation_path(a, 10) +// right(80) cube([10,10,1]); +// #sphere(r=80); +function Q_Rotation_path(q1, n=1, q2) = + assert( Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2) ), "Invalid quaternion(s)" ) + assert( is_finite(n) && n>=1 && n==floor(n), "Invalid integer" ) + assert( is_undef(q2) || ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" ) + is_undef(q2) + ? [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 ), + dq = Q_pow( Q_Mul( q2, Q_Inverse(q1) ), 1/n ), + dR = Q_Matrix4(dq) ) + [for( i=0, R=Q_Matrix4(q1); i<=n; i=i+1, R=dR*R ) R]; + +module Q_Rotation_path(q1, n=1, q2) { + for(Mi=Q_Rotation_path(q1, n, q2)) + multmatrix(Mi) + children(); +} + + +// Function: Q_Nlerp() +// Usage: +// q = Q_Nlerp(q1, q2, u); +// Description: +// Returns a quaternion that is a normalized linear interpolation between two quaternions +// when u is a number. +// If u is a list of numbers, computes the interpolations for each value in the +// list and returns the interpolated quaternions in a list. +// The input quaternions don't need to be normalized. +// Arguments: +// q1 = The first quaternion. (u=0) +// q2 = The second quaternion. (u=1) +// u = A value (or a list of values), between 0 and 1, of the proportion(s) of each quaternion in the interpolation. +// Example(3D): Giving `u` as a Scalar +// a = QuatY(-135); +// b = QuatXYZ([0,-30,30]); +// for (u=[0:0.1:1]) +// Qrot(Q_Nlerp(a, b, u)) +// right(80) cube([10,10,1]); +// #sphere(r=80); +// Example(3D): Giving `u` as a Range +// a = QuatZ(-135); +// b = QuatXYZ([90,0,-45]); +// for (q = Q_Nlerp(a, b, [0:0.1:1])) +// Qrot(q) right(80) cube([10,10,1]); +// #sphere(r=80); +function Q_Nlerp(q1,q2,u) = + 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( ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" ) + let( q1 = Q_Normalize(q1), + q2 = Q_Normalize(q2) ) + is_num(u) + ? _Qnorm((1-u)*q1 + u*q2 ) + : [for (ui=u) _Qnorm((1-ui)*q1 + ui*q2 ) ]; + + +// Function: Q_Squad() +// Usage: +// qn = Q_Squad(q1,q2,q3,q4,u); +// Description: +// Returns a quaternion that is a cubic spherical interpolation of the quaternions +// q1 and q4 taking the other two quaternions, q2 and q3, as parameter of a cubic +// on the sphere similar to the control points of a Bezier curve. +// If u is a number, usually between 0 and 1, returns the quaternion that results +// from the interpolation. +// If u is a list of numbers, computes the interpolations for each value in the +// list and returns the interpolated quaternions in a list. +// The input quaternions don't need to be normalized. +// Arguments: +// q1 = The start quaternion. (u=0) +// q1 = The first intermediate quaternion. +// q2 = The second intermediate quaternion. +// q4 = The end quaternion. (u=1) +// u = A value (or a list of values), of the proportion(s) of each quaternion in the cubic interpolation. +// Example(3D): Giving `u` as a Scalar +// a = QuatY(-135); +// b = QuatXYZ([-50,-50,120]); +// c = QuatXYZ([-50,-40,30]); +// d = QuatY(-45); +// color("red"){ +// Qrot(b) right(80) cube([10,10,1]); +// Qrot(c) right(80) cube([10,10,1]); +// } +// for (u=[0:0.05:1]) +// Qrot(Q_Squad(a, b, c, d, u)) +// right(80) cube([10,10,1]); +// #sphere(r=80); +// Example(3D): Giving `u` as a Range +// a = QuatY(-135); +// b = QuatXYZ([-50,-50,120]); +// c = QuatXYZ([-50,-40,30]); +// d = QuatY(-45); +// for (q = Q_Squad(a, b, c, d, [0:0.05:1])) +// Qrot(q) right(80) cube([10,10,1]); +// #sphere(r=80); +function Q_Squad(q1,q2,q3,q4,u) = + assert(is_finite(u) || is_range(u) || is_vector(u) , + "Invalid interpolation coefficient(s)" ) + is_num(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) ) ]; + + +// Function: Q_exp() +// Usage: +// q2 = Q_exp(q); +// Description: +// Returns the quaternion that is the exponential of the quaternion q in base e +// The returned quaternion is usually not normalized. +function Q_exp(q) = + assert( is_vector(q,4), "Input is not a valid quaternion") + let( nv = norm(_Qvec(q)) ) // q may be equal to zero here! + exp(_Qreal(q))*Quat(_Qvec(q),2*nv); + + +// Function: Q_ln() +// Usage: +// q2 = Q_ln(q); +// Description: +// Returns the quaternion that is the natural logarithm of the quaternion q. +// The returned quaternion is usually not normalized and may be zero. +function Q_ln(q) = + assert(Q_is_quat(q), "Input is not a valid quaternion") + let( nq = norm(q), + nv = norm(_Qvec(q)) ) + approx(nv,0) ? _Qset([0,0,0] , ln(nq) ) : + _Qset(_Qvec(q)*atan2(nv,_Qreal(q))/nv, ln(nq)); + + +// Function: Q_pow() +// Usage: +// q2 = Q_pow(q, r); +// Description: +// 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. +function Q_pow(q,r=1) = + assert( Q_is_quat(q) && is_finite(r), + "Invalid inputs") + let( theta = 2*atan2(norm(_Qvec(q)),_Qreal(q)) ) + Quat(_Qvec(q), r*theta); // Q_exp(r*Q_ln(q)); + + + // vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap diff --git a/tests/test_quaternions.scad b/tests/test_quaternions.scad index b9b232f..7a66d25 100644 --- a/tests/test_quaternions.scad +++ b/tests/test_quaternions.scad @@ -8,6 +8,7 @@ function rec_cmp(a,b,eps=1e-9) = is_list(a)? len(a)==len(b) && all([for (i=idx(a)) rec_cmp(a[i],b[i],eps=eps)]) : a == b; +function Qstandard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q; module verify_f(actual,expected) { if (!rec_cmp(actual,expected)) { @@ -22,6 +23,15 @@ module verify_f(actual,expected) { } +module test_is_quat() { + verify_f(Q_is_quat([0]),false); + verify_f(Q_is_quat([0,0,0,0]),false); + verify_f(Q_is_quat([1,0,2,0]),true); + verify_f(Q_is_quat([1,0,2,0,0]),false); +} +test_is_quat(); + + module test_Quat() { verify_f(Quat(UP,0),[0,0,0,1]); verify_f(Quat(FWD,0),[0,0,0,1]); @@ -92,6 +102,15 @@ module test_QuatXYZ() { test_QuatXYZ(); +module test_Q_From_to() { + verify_f(Q_Mul(Q_From_to([1,2,3], [4,5,2]),Q_From_to([4,5,2], [1,2,3])), Q_Ident()); + verify_f(Q_Matrix4(Q_From_to([1,2,3], [4,5,2])), rot(from=[1,2,3],to=[4,5,2])); + verify_f(Qrot(Q_From_to([1,2,3], -[1,2,3]),[1,2,3]), -[1,2,3]); + verify_f(unit(Qrot(Q_From_to([1,2,3], [4,5,2]),[1,2,3])), unit([4,5,2])); +} +test_Q_From_to(); + + module test_Q_Ident() { verify_f(Q_Ident(), [0,0,0,1]); } @@ -207,6 +226,16 @@ module test_Q_Conj() { test_Q_Conj(); +module test_Q_Inverse() { + + verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]/sqrt(2)); + verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]/sqrt(2)); + verify_f(Q_Inverse(QuatXYZ([23,45,67])),Q_Conj(QuatXYZ([23,45,67]))); + verify_f(Q_Mul(Q_Inverse(QuatXYZ([23,45,67])),QuatXYZ([23,45,67])),Q_Ident()); +} +test_Q_Inverse(); + + module test_Q_Norm() { verify_f(Q_Norm([1,0,0,1]),1.414213562); verify_f(Q_Norm([0,1,1,0]),1.414213562); @@ -276,6 +305,10 @@ module test_Q_Angle() { verify_f(Q_Angle(QuatY(-37)),37); verify_f(Q_Angle(QuatZ(37)),37); verify_f(Q_Angle(QuatZ(-37)),37); + + verify_f(Q_Angle(QuatZ(-37),QuatZ(-37)), 0); + verify_f(Q_Angle(QuatZ( 37.123),QuatZ(-37.123)), 74.246); + verify_f(Q_Angle(QuatX( 37),QuatY(-37)), 51.86293283); } test_Q_Angle(); @@ -288,4 +321,87 @@ module test_Qrot() { test_Qrot(); +module test_Q_Rotation() { + verify_f(Qstandard(Q_Rotation(Q_Matrix3(Quat([12,34,56],33)))),Qstandard(Quat([12,34,56],33))); + verify_f(Q_Matrix3(Q_Rotation(Q_Matrix3(QuatXYZ([12,34,56])))), + Q_Matrix3(QuatXYZ([12,34,56]))); +} +test_Q_Rotation(); + + +module test_Q_Rotation_path() { + + verify_f(Q_Rotation_path(QuatX(135), 5, QuatY(13.5))[0] , Q_Matrix4(QuatX(135))); + verify_f(Q_Rotation_path(QuatX(135), 11, QuatY(13.5))[11] , yrot(13.5)); + verify_f(Q_Rotation_path(QuatX(135), 16, QuatY(13.5))[8] , Q_Rotation_path(QuatX(135), 8, QuatY(13.5))[4]); + verify_f(Q_Rotation_path(QuatX(135), 16, QuatY(13.5))[7] , + Q_Rotation_path(QuatY(13.5),16, QuatX(135))[9]); + + verify_f(Q_Rotation_path(QuatX(11), 5)[0] , xrot(11)); + verify_f(Q_Rotation_path(QuatX(11), 5)[4] , xrot(55)); + +} +test_Q_Rotation_path(); + + +module test_Q_Nlerp() { + verify_f(Q_Nlerp(QuatX(45),QuatY(30),0.0),QuatX(45)); + verify_f(Q_Nlerp(QuatX(45),QuatY(30),0.5),[0.1967063121, 0.1330377423, 0, 0.9713946602]); + verify_f(Q_Rotation_path(QuatX(135), 16, QuatY(13.5))[8] , Q_Matrix4(Q_Nlerp(QuatX(135), QuatY(13.5),0.5))); + verify_f(Q_Nlerp(QuatX(45),QuatY(30),1.0),QuatY(30)); +} +test_Q_Nlerp(); + + +module test_Q_Squad() { + verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(90),QuatY(30),0.0),QuatX(45)); + verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(90),QuatY(30),1.0),QuatY(30)); + verify_f(Q_Squad(QuatX(0),QuatX(30),QuatX(90),QuatX(120),0.5), + Q_Slerp(QuatX(0),QuatX(120),0.5)); + verify_f(Q_Squad(QuatY(0),QuatY(0),QuatX(120),QuatX(120),0.3), + Q_Slerp(QuatY(0),QuatX(120),0.3)); +} +test_Q_Squad(); + + +module test_Q_exp() { + verify_f(Q_exp(Q_Ident()), exp(1)*Q_Ident()); + verify_f(Q_exp([0,0,0,33.7]), exp(33.7)*Q_Ident()); + verify_f(Q_exp(Q_ln(Q_Ident())), Q_Ident()); + verify_f(Q_exp(Q_ln([1,2,3,0])), [1,2,3,0]); + verify_f(Q_exp(Q_ln(QuatXYZ([31,27,34]))), QuatXYZ([31,27,34])); + let(q=QuatXYZ([12,23,34])) + verify_f(Q_exp(q+Q_Inverse(q)),Q_Mul(Q_exp(q),Q_exp(Q_Inverse(q)))); + +} +test_Q_exp(); + + +module test_Q_ln() { + verify_f(Q_ln([1,2,3,0]), [24.0535117721, 48.1070235442, 72.1605353164, 1.31952866481]); + verify_f(Q_ln(Q_Ident()), [0,0,0,0]); + verify_f(Q_ln(5.5*Q_Ident()), [0,0,0,ln(5.5)]); + verify_f(Q_ln(Q_exp(QuatXYZ([13,37,43]))), QuatXYZ([13,37,43])); + verify_f(Q_ln(QuatXYZ([12,23,34]))+Q_ln(Q_Inverse(QuatXYZ([12,23,34]))), [0,0,0,0]); +} +test_Q_ln(); + + +module test_Q_pow() { + q = Quat([1,2,3],77); + verify_f(Q_pow(q,1), q); + verify_f(Q_pow(q,0), Q_Ident()); + verify_f(Q_pow(q,-1), Q_Inverse(q)); + verify_f(Q_pow(q,2), Q_Mul(q,q)); + verify_f(Q_pow(q,3), Q_Mul(q,Q_pow(q,2))); + verify_f(Q_Mul(Q_pow(q,0.456),Q_pow(q,0.544)), q); + verify_f(Q_Mul(Q_pow(q,0.335),Q_Mul(Q_pow(q,.552),Q_pow(q,.113))), q); +} +test_Q_pow(); + + + + + + // vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap