From 3eb506e78ab6ed70b60340428658229c546af2ef Mon Sep 17 00:00:00 2001 From: Garth Minette Date: Sat, 12 Jun 2021 17:17:05 -0700 Subject: [PATCH] Rename quaternion functions to not have uppercase names. --- paths.scad | 14 +- quaternions.scad | 483 +++++++++++++++---------------- shapes2d.scad | 6 +- tests/test_quaternions.scad | 553 +++++++++++++++++------------------- 4 files changed, 519 insertions(+), 537 deletions(-) diff --git a/paths.scad b/paths.scad index 1dc1c2f..73ed488 100644 --- a/paths.scad +++ b/paths.scad @@ -1109,11 +1109,11 @@ module spiral_sweep(poly, h, r, twist=360, higbee, center, r1, r2, d, d1, d2, hi // path = [ [0, 0, 0], [33, 33, 33], [66, 33, 40], [100, 0, 0], [150,0,0] ]; // path_extrude(path) circle(r=10, $fn=6); module path_extrude(path, convexity=10, clipsize=100) { - function polyquats(path, q=Q_Ident(), v=[0,0,1], i=0) = let( + function polyquats(path, q=q_ident(), v=[0,0,1], i=0) = let( v2 = path[i+1] - path[i], ang = vector_angle(v,v2), axis = ang>0.001? unit(cross(v,v2)) : [0,0,1], - newq = Q_Mul(Quat(axis, ang), q), + newq = q_mul(quat(axis, ang), q), dist = norm(v2) ) i < (len(path)-2)? concat([[dist, newq, ang]], polyquats(path, newq, v2, i+1)) : @@ -1129,7 +1129,7 @@ module path_extrude(path, convexity=10, clipsize=100) { q = pquats[i][1]; difference() { translate(pt1) { - Qrot(q) { + q_rot(q) { down(clipsize/2/2) { if ((dist+clipsize/2) > 0) { linear_extrude(height=dist+clipsize/2, convexity=convexity) { @@ -1140,12 +1140,12 @@ module path_extrude(path, convexity=10, clipsize=100) { } } translate(pt1) { - hq = (i > 0)? Q_Slerp(q, pquats[i-1][1], 0.5) : q; - Qrot(hq) down(clipsize/2+epsilon) cube(clipsize, center=true); + hq = (i > 0)? q_slerp(q, pquats[i-1][1], 0.5) : q; + q_rot(hq) down(clipsize/2+epsilon) cube(clipsize, center=true); } translate(pt2) { - hq = (i < ptcount-2)? Q_Slerp(q, pquats[i+1][1], 0.5) : q; - Qrot(hq) up(clipsize/2+epsilon) cube(clipsize, center=true); + hq = (i < ptcount-2)? q_slerp(q, pquats[i+1][1], 0.5) : q; + q_rot(hq) up(clipsize/2+epsilon) cube(clipsize, center=true); } } } diff --git a/quaternions.scad b/quaternions.scad index 1dc7192..7598655 100644 --- a/quaternions.scad +++ b/quaternions.scad @@ -16,190 +16,190 @@ // Internal -function _Quat(a,s,w) = [a[0]*s, a[1]*s, a[2]*s, w]; +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 _qvec(q) = [q.x,q.y,q.z]; -function _Qreal(q) = q[3]; +function _qreal(q) = q[3]; -function _Qset(v,r) = concat( v, r ); +function _qset(v,r) = concat( v, r ); // normalizes without checking -function _Qnorm(q) = q/norm(q); +function _qnorm(q) = q/norm(q); -// Function: Q_is_quat() +// Function: is_quaternion() // Usage: -// if(Q_is_quat(q)) a=0; +// if(is_quaternion(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 is_quaternion(q) = is_vector(q,4) && ! approx(norm(q),0) ; -// Function: Quat() +// Function: quat() // Usage: -// Quat(ax, ang); +// quat(ax, ang); // 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) = +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)); + ? _quat([0,0,0], sin(ang/2), cos(ang/2)) + : _quat(ax/n, sin(ang/2), cos(ang/2)); -// Function: QuatX() +// Function: quat_x() // Usage: -// QuatX(a); +// quat_x(a); // 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) = +function quat_x(a=0) = assert( is_finite(a), "Invalid angle" ) - Quat([1,0,0],a); + quat([1,0,0],a); -// Function: QuatY() +// Function: quat_y() // Usage: -// QuatY(a); +// quat_y(a); // 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) = +function quat_y(a=0) = assert( is_finite(a), "Invalid angle" ) - Quat([0,1,0],a); + quat([0,1,0],a); -// Function: QuatZ() +// Function: quat_z() // Usage: -// QuatZ(a); +// quat_z(a); // 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) = +function quat_z(a=0) = assert( is_finite(a), "Invalid angle" ) - Quat([0,0,1],a); + quat([0,0,1],a); -// Function: QuatXYZ() +// Function: quat_xyz() // Usage: -// QuatXYZ([X,Y,Z]) +// quat_xyz([X,Y,Z]) // Description: // 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]) = +function quat_xyz(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 = quat_x(a[0]), + qy = quat_y(a[1]), + qz = quat_z(a[2]) ) - Q_Mul(qz, Q_Mul(qy, qx)); + q_mul(qz, q_mul(qy, qx)); -// Function: Q_From_to() +// Function: q_from_to() // Usage: -// q = Q_From_to(v1, v2); +// 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) = +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 )); + ? v1*v2>0 ? q_ident() : quat([ v1.y, -v1.x, 0], 180) + : quat(ax, atan2( n , v1*v2 )); -// Function: Q_Ident() +// Function: q_ident() // Description: Returns the "Identity" zero-rotation Quaternion. -function Q_Ident() = [0, 0, 0, 1]; +function q_ident() = [0, 0, 0, 1]; -// Function: Q_Add_S() +// Function: q_add_s() // Usage: -// Q_Add_S(q, s) +// q_add_s(q, 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) = +function q_add_s(q, s) = assert( is_finite(s), "Invalid scalar" ) q+[0,0,0,s]; -// Function: Q_Sub_S() +// Function: q_sub_s() // Usage: -// Q_Sub_S(q, s) +// q_sub_s(q, 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) = +function q_sub_s(q, s) = assert( is_finite(s), "Invalid scalar" ) q-[0,0,0,s]; -// Function: Q_Mul_S() +// Function: q_mul_s() // Usage: -// Q_Mul_S(q, s) +// q_mul_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) = +function q_mul_s(q, s) = assert( is_finite(s), "Invalid scalar" ) q*s; -// Function: Q_Div_S() +// Function: q_div_s() // Usage: -// Q_Div_S(q, s) +// q_div_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) = +function q_div_s(q, s) = assert( is_finite(s) && ! approx(s,0) , "Invalid scalar" ) q/s; -// Function: Q_Add() +// Function: q_add() // Usage: -// Q_Add(a, b) +// q_add(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)") +function q_add(a, b) = + assert( is_quaternion(a) && is_quaternion(a), "Invalid quaternion(s)") assert( ! approx(norm(a+b),0), "Quaternions cannot be opposed" ) a+b; -// Function: Q_Sub() +// Function: q_sub() // Usage: -// Q_Sub(a, b) +// q_sub(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)") +function q_sub(a, b) = + assert( is_quaternion(a) && is_quaternion(a), "Invalid quaternion(s)") assert( ! approx(a,b), "Quaternions cannot be equal" ) a-b; -// Function: Q_Mul() +// Function: q_mul() // Usage: -// Q_Mul(a, b) +// q_mul(a, b) // 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)") +function q_mul(a, b) = + assert( is_quaternion(a) && is_quaternion(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, @@ -208,94 +208,94 @@ function Q_Mul(a, b) = ]; -// Function: Q_Cumulative() +// Function: q_cumulative() // Usage: -// Q_Cumulative(v); +// q_cumulative(v); // Description: // 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=[]) = +function q_cumulative(v, _i=0, _acc=[]) = _i==len(v) ? _acc : - Q_Cumulative( + q_cumulative( v, _i+1, concat( _acc, - [_i==0 ? v[_i] : Q_Mul(v[_i], last(_acc))] + [_i==0 ? v[_i] : q_mul(v[_i], last(_acc))] ) ); -// Function: Q_Dot() +// Function: q_dot() // Usage: -// Q_Dot(a, b) +// q_dot(a, b) // Description: Calculates the dot product between quaternions `a` and `b`. -function Q_Dot(a, b) = - assert( Q_is_quat(a) && Q_is_quat(b), "Invalid quaternion(s)" ) +function q_dot(a, b) = + assert( is_quaternion(a) && is_quaternion(b), "Invalid quaternion(s)" ) a*b; -// Function: Q_Neg() +// Function: q_neg() // Usage: -// Q_Neg(q) +// q_neg(q) // Description: Returns the negative of quaternion `q`. -function Q_Neg(q) = - assert( Q_is_quat(q), "Invalid quaternion" ) +function q_neg(q) = + assert( is_quaternion(q), "Invalid quaternion" ) -q; -// Function: Q_Conj() +// Function: q_conj() // Usage: -// Q_Conj(q) +// q_conj(q) // Description: Returns the conjugate of quaternion `q`. -function Q_Conj(q) = - assert( Q_is_quat(q), "Invalid quaternion" ) +function q_conj(q) = + assert( is_quaternion(q), "Invalid quaternion" ) [-q.x, -q.y, -q.z, q[3]]; -// Function: Q_Inverse() +// Function: q_inverse() // Usage: -// qc = Q_Inverse(q) +// 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) ) +function q_inverse(q) = + assert( is_quaternion(q), "Invalid quaternion" ) + let(q = _qnorm(q) ) [-q.x, -q.y, -q.z, q[3]]; -// Function: Q_Norm() +// Function: q_norm() // Usage: -// Q_Norm(q) +// 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" ) +function q_norm(q) = + assert( is_quaternion(q), "Invalid quaternion" ) norm(q); -// Function: Q_Normalize() +// Function: q_normalize() // Usage: -// Q_Normalize(q) +// q_normalize(q) // Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1. -function Q_Normalize(q) = - assert( Q_is_quat(q) , "Invalid quaternion" ) +function q_normalize(q) = + assert( is_quaternion(q) , "Invalid quaternion" ) q/norm(q); -// Function: Q_Dist() +// Function: q_dist() // Usage: -// Q_Dist(q1, q2) +// q_dist(q1, q2) // Description: Returns the "distance" between two quaternions. -function Q_Dist(q1, q2) = - assert( Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) +function q_dist(q1, q2) = + assert( is_quaternion(q1) && is_quaternion(q2), "Invalid quaternion(s)" ) norm(q2-q1); -// Function: Q_Slerp() +// Function: q_slerp() // Usage: -// Q_Slerp(q1, q2, u); +// q_slerp(q1, q2, u); // Description: // Returns a quaternion that is a spherical interpolation between two quaternions. // Arguments: @@ -303,45 +303,45 @@ function Q_Dist(q1, q2) = // q2 = The second quaternion. (u=1) // u = The proportional value, from 0 to 1, of what part of the interpolation to return. // Example(3D): Giving `u` as a Scalar -// a = QuatY(-135); -// b = QuatXYZ([0,-30,30]); +// a = quat_y(-135); +// b = quat_xyz([0,-30,30]); // for (u=[0:0.1:1]) -// Qrot(Q_Slerp(a, b, u)) +// q_rot(q_slerp(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_Slerp(a, b, [0:0.1:1])) -// Qrot(q) right(80) cube([10,10,1]); +// a = quat_z(-135); +// b = quat_xyz([90,0,-45]); +// for (q = q_slerp(a, b, [0:0.1:1])) +// q_rot(q) right(80) cube([10,10,1]); // #sphere(r=80); -function Q_Slerp(q1, q2, u, _dot) = +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)" ) + assert(is_quaternion(q1) && is_quaternion(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) + ! 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)) + ? _qnorm(q1 + u*(q2-q1)) : let( theta = u*acos(_dot), - q3 = _Qnorm(q2 - _dot*q1) + 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() // Usage: -// Q_Matrix3(q); +// q_matrix3(q); // Description: // Returns the 3x3 rotation matrix for the given normalized quaternion q. -function Q_Matrix3(q) = - let( q = Q_Normalize(q) ) +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]], @@ -349,13 +349,13 @@ function Q_Matrix3(q) = ]; -// Function: Q_Matrix4() +// Function: q_matrix4() // Usage: -// Q_Matrix4(q); +// q_matrix4(q); // Description: // Returns the 4x4 rotation matrix for the given normalized quaternion q. -function Q_Matrix4(q) = - let( q = Q_Normalize(q) ) +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], @@ -364,115 +364,115 @@ function Q_Matrix4(q) = ]; -// Function: Q_Axis() +// Function: q_axis() // Usage: -// Q_Axis(q) +// q_axis(q) // Description: // Returns the axis of rotation of a normalized quaternion `q`. // 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_axis(q) = + assert( is_quaternion(q) , "Invalid quaternion" ) + let( d = norm(_qvec(q)) ) + approx(d,0)? [0,0,1] : _qvec(q)/d; -// Function: Q_Angle() +// Function: q_angle() // Usage: -// a = Q_Angle(q) -// a12 = Q_Angle(q1,q2); +// a = q_angle(q) +// a12 = q_angle(q1,q2); // Description: // 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) ) +function q_angle(q1,q2) = + assert(is_quaternion(q1) && (is_undef(q2) || is_quaternion(q2)), "Invalid quaternion(s)" ) + let( n1 = is_undef(q2)? norm(_qvec(q1)): norm(q1) ) is_undef(q2) - ? 2 * atan2(n1,_Qreal(q1)) + ? 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() +// Function&Module: q_rot() // Usage: As Module -// Qrot(q) ... +// q_rot(q) ... // Usage: As Function -// pts = Qrot(q,p); +// pts = q_rot(q,p); // Description: // When called as a module, rotates all children by the rotation stored in quaternion `q`. // When called as a function with a `p` argument, rotates the point or list of points in `p` by the rotation stored in quaternion `q`. // When called as a function without a `p` argument, returns the affine3d rotation matrix for the rotation stored in quaternion `q`. // Example(FlatSpin,VPD=225,VPT=[71,-26,16]): // module shape() translate([80,0,0]) cube([10,10,1]); -// q = QuatXYZ([90,-15,-45]); -// Qrot(q) shape(); +// q = quat_xyz([90,-15,-45]); +// q_rot(q) shape(); // #shape(); // Example(NORENDER): -// q = QuatXYZ([45,35,10]); -// mat4x4 = Qrot(q); +// q = quat_xyz([45,35,10]); +// mat4x4 = q_rot(q); // Example(NORENDER): -// q = QuatXYZ([45,35,10]); -// pt = Qrot(q, p=[4,5,6]); +// q = quat_xyz([45,35,10]); +// pt = q_rot(q, p=[4,5,6]); // Example(NORENDER): -// q = QuatXYZ([45,35,10]); -// pts = Qrot(q, p=[[2,3,4], [4,5,6], [9,2,3]]); -module Qrot(q) { - multmatrix(Q_Matrix4(q)) { +// q = quat_xyz([45,35,10]); +// pts = q_rot(q, p=[[2,3,4], [4,5,6], [9,2,3]]); +module q_rot(q) { + multmatrix(q_matrix4(q)) { children(); } } -function Qrot(q,p) = - is_undef(p)? Q_Matrix4(q) : - is_vector(p)? Qrot(q,[p])[0] : - apply(Q_Matrix4(q), p); +function q_rot(q,p) = + is_undef(p)? q_matrix4(q) : + is_vector(p)? q_rot(q,[p])[0] : + apply(q_matrix4(q), p); -// Module: Qrot_copies() +// Module: q_rot_copies() // Usage: -// Qrot_copies(quats) ... +// q_rot_copies(quats) ... // Description: // For each quaternion given in the list `quats`, rotates to that orientation and creates a copy -// of all children. This is equivalent to `for (q=quats) Qrot(q) ...`. +// of all children. This is equivalent to `for (q=quats) q_rot(q) ...`. // Arguments: // quats = A list containing all quaternions to rotate to and create copies of all children for. // Example: -// a = QuatZ(-135); -// b = QuatXYZ([0,-30,30]); -// Qrot_copies(Q_Slerp(a, b, [0:0.1:1])) +// a = quat_z(-135); +// b = quat_xyz([0,-30,30]); +// q_rot_copies(q_slerp(a, b, [0:0.1:1])) // right(80) cube([10,10,1]); // #sphere(r=80); -module Qrot_copies(quats) for (q=quats) Qrot(q) children(); +module q_rot_copies(quats) for (q=quats) q_rot(q) children(); -// Function: Q_Rotation() +// Function: q_rotation() // Usage: -// Q_Rotation(R) +// 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) = +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 ) ) + _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])) ) ; + 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() +// Function&Module: q_rotation_path() // Usage: As a function -// path = Q_Rotation_path(q1, n, q2); -// path = Q_Rotation_path(q1, n); +// path = q_rotation_path(q1, n, q2); +// path = q_rotation_path(q1, n); // Usage: As a module -// Q_Rotation_path(q1, n, q2) ... +// 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. @@ -488,50 +488,50 @@ function Q_Rotation(R) = // 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)) +// a = quat_y(-135); +// b = quat_xyz([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) +// a = quat_y(-135); +// b = quat_xyz([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)) +// a = quat_y(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) +// a = quat_y(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)" ) +function q_rotation_path(q1, n=1, q2) = + assert( is_quaternion(q1) && (is_undef(q2) || is_quaternion(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]; + ? [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)) +module q_rotation_path(q1, n=1, q2) { + for(Mi=q_rotation_path(q1, n, q2)) multmatrix(Mi) children(); } -// Function: Q_Nlerp() +// Function: q_nlerp() // Usage: -// q = Q_Nlerp(q1, q2, u); +// q = q_nlerp(q1, q2, u); // Description: // Returns a quaternion that is a normalized linear interpolation between two quaternions // when u is a number. @@ -543,33 +543,33 @@ module Q_Rotation_path(q1, n=1, q2) { // 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]); +// a = quat_y(-135); +// b = quat_xyz([0,-30,30]); // for (u=[0:0.1:1]) -// Qrot(Q_Nlerp(a, b, u)) +// q_rot(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]); +// a = quat_z(-135); +// b = quat_xyz([90,0,-45]); +// for (q = q_nlerp(a, b, [0:0.1:1])) +// q_rot(q) right(80) cube([10,10,1]); // #sphere(r=80); -function Q_Nlerp(q1,q2,u) = +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(is_quaternion(q1) && is_quaternion(q2), "Invalid quaternion(s)" ) assert( ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" ) - let( q1 = Q_Normalize(q1), - q2 = Q_Normalize(q2) ) + 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 ) ]; + ? _qnorm((1-u)*q1 + u*q2 ) + : [for (ui=u) _qnorm((1-ui)*q1 + ui*q2 ) ]; -// Function: Q_Squad() +// Function: q_squad() // Usage: -// qn = Q_Squad(q1,q2,q3,q4,u); +// 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 @@ -586,71 +586,72 @@ function Q_Nlerp(q1,q2,u) = // 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); +// a = quat_y(-135); +// b = quat_xyz([-50,-50,120]); +// c = quat_xyz([-50,-40,30]); +// d = quat_y(-45); // color("red"){ -// Qrot(b) right(80) cube([10,10,1]); -// Qrot(c) right(80) cube([10,10,1]); +// q_rot(b) right(80) cube([10,10,1]); +// q_rot(c) right(80) cube([10,10,1]); // } // for (u=[0:0.05:1]) -// Qrot(Q_Squad(a, b, c, d, u)) +// q_rot(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]); +// a = quat_y(-135); +// b = quat_xyz([-50,-50,120]); +// c = quat_xyz([-50,-40,30]); +// d = quat_y(-45); +// for (q = q_squad(a, b, c, d, [0:0.05:1])) +// q_rot(q) right(80) cube([10,10,1]); // #sphere(r=80); -function Q_Squad(q1,q2,q3,q4,u) = +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) ) ]; + ? 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() +// Function: q_exp() // Usage: -// q2 = Q_exp(q); +// 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) = +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); + let( nv = norm(_qvec(q)) ) // q may be equal to zero here! + exp(_qreal(q))*quat(_qvec(q),2*nv); -// Function: Q_ln() +// Function: q_ln() // Usage: -// q2 = Q_ln(q); +// 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_ln(q) = + assert(is_quaternion(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() +// Function: q_pow() // Usage: -// q2 = Q_pow(q, r); +// 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)); +function q_pow(q,r=1) = + assert( is_quaternion(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)); diff --git a/shapes2d.scad b/shapes2d.scad index f24fa0c..1fce85f 100644 --- a/shapes2d.scad +++ b/shapes2d.scad @@ -307,15 +307,15 @@ module stroke( multmatrix(mat) polygon(endcap_shape2); } } else { - quatsums = Q_Cumulative([ + quatsums = q_cumulative([ for (i = idx(path2,e=-2)) let( vec1 = i==0? UP : unit(path2[i]-path2[i-1], UP), vec2 = unit(path2[i+1]-path2[i], UP), axis = vector_axis(vec1,vec2), ang = vector_angle(vec1,vec2) - ) Quat(axis,ang) + ) quat(axis,ang) ]); - rotmats = [for (q=quatsums) Q_Matrix4(q)]; + rotmats = [for (q=quatsums) q_matrix4(q)]; sides = [ for (i = idx(path2,e=-2)) quantup(segs(max(widths[i],widths[i+1])/2),4) diff --git a/tests/test_quaternions.scad b/tests/test_quaternions.scad index b7c2f6f..fe95ebc 100644 --- a/tests/test_quaternions.scad +++ b/tests/test_quaternions.scad @@ -1,403 +1,384 @@ include <../std.scad> -include <../strings.scad> -function rec_cmp(a,b,eps=1e-9) = - typeof(a)!=typeof(b)? false : - is_num(a)? approx(a,b,eps=eps) : - 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; +function _q_standard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q; -module verify_f(actual,expected) { - if (!rec_cmp(actual,expected)) { - echo(str("Expected: ",fmt_float(expected,10))); - echo(str(" : ",expected)); - echo(str("Actual : ",fmt_float(actual,10))); - echo(str(" : ",actual)); - echo(str("Delta : ",fmt_float(expected-actual,10))); - echo(str(" : ",expected-actual)); - assert(approx(expected,actual)); - } + +module test_is_quaternion() { + assert_approx(is_quaternion([0]),false); + assert_approx(is_quaternion([0,0,0,0]),false); + assert_approx(is_quaternion([1,0,2,0]),true); + assert_approx(is_quaternion([1,0,2,0,0]),false); } +test_is_quaternion(); -module test_Q_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); +module test_quat() { + assert_approx(quat(UP,0),[0,0,0,1]); + assert_approx(quat(FWD,0),[0,0,0,1]); + assert_approx(quat(LEFT,0),[0,0,0,1]); + assert_approx(quat(UP,45),[0,0,0.3826834324,0.9238795325]); + assert_approx(quat(LEFT,45),[-0.3826834324, 0, 0, 0.9238795325]); + assert_approx(quat(BACK,45),[0,0.3826834323,0,0.9238795325]); + assert_approx(quat(FWD+RIGHT,30),[0.1830127019, -0.1830127019, 0, 0.9659258263]); } -test_Q_is_quat(); +test_quat(); -module test_Quat() { - verify_f(Quat(UP,0),[0,0,0,1]); - verify_f(Quat(FWD,0),[0,0,0,1]); - verify_f(Quat(LEFT,0),[0,0,0,1]); - verify_f(Quat(UP,45),[0,0,0.3826834324,0.9238795325]); - verify_f(Quat(LEFT,45),[-0.3826834324, 0, 0, 0.9238795325]); - verify_f(Quat(BACK,45),[0,0.3826834323,0,0.9238795325]); - verify_f(Quat(FWD+RIGHT,30),[0.1830127019, -0.1830127019, 0, 0.9659258263]); +module test_quat_x() { + assert_approx(quat_x(0),[0,0,0,1]); + assert_approx(quat_x(35),[0.3007057995,0,0,0.9537169507]); + assert_approx(quat_x(45),[0.3826834324,0,0,0.9238795325]); } -test_Quat(); +test_quat_x(); -module test_QuatX() { - verify_f(QuatX(0),[0,0,0,1]); - verify_f(QuatX(35),[0.3007057995,0,0,0.9537169507]); - verify_f(QuatX(45),[0.3826834324,0,0,0.9238795325]); +module test_quat_y() { + assert_approx(quat_y(0),[0,0,0,1]); + assert_approx(quat_y(35),[0,0.3007057995,0,0.9537169507]); + assert_approx(quat_y(45),[0,0.3826834323,0,0.9238795325]); } -test_QuatX(); +test_quat_y(); -module test_QuatY() { - verify_f(QuatY(0),[0,0,0,1]); - verify_f(QuatY(35),[0,0.3007057995,0,0.9537169507]); - verify_f(QuatY(45),[0,0.3826834323,0,0.9238795325]); +module test_quat_z() { + assert_approx(quat_z(0),[0,0,0,1]); + assert_approx(quat_z(36),[0,0,0.3090169944,0.9510565163]); + assert_approx(quat_z(45),[0,0,0.3826834324,0.9238795325]); } -test_QuatY(); +test_quat_z(); -module test_QuatZ() { - verify_f(QuatZ(0),[0,0,0,1]); - verify_f(QuatZ(36),[0,0,0.3090169944,0.9510565163]); - verify_f(QuatZ(45),[0,0,0.3826834324,0.9238795325]); +module test_quat_xyz() { + assert_approx(quat_xyz([0,0,0]), [0,0,0,1]); + assert_approx(quat_xyz([30,0,0]), [0.2588190451, 0, 0, 0.9659258263]); + assert_approx(quat_xyz([90,0,0]), [0.7071067812, 0, 0, 0.7071067812]); + assert_approx(quat_xyz([-270,0,0]), [-0.7071067812, 0, 0, -0.7071067812]); + assert_approx(quat_xyz([180,0,0]), [1,0,0,0]); + assert_approx(quat_xyz([270,0,0]), [0.7071067812, 0, 0, -0.7071067812]); + assert_approx(quat_xyz([-90,0,0]), [-0.7071067812, 0, 0, 0.7071067812]); + assert_approx(quat_xyz([360,0,0]), [0,0,0,-1]); + + assert_approx(quat_xyz([0,0,0]), [0,0,0,1]); + assert_approx(quat_xyz([0,30,0]), [0, 0.2588190451, 0, 0.9659258263]); + assert_approx(quat_xyz([0,90,0]), [0, 0.7071067812, 0, 0.7071067812]); + assert_approx(quat_xyz([0,-270,0]), [0, -0.7071067812, 0, -0.7071067812]); + assert_approx(quat_xyz([0,180,0]), [0,1,0,0]); + assert_approx(quat_xyz([0,270,0]), [0, 0.7071067812, 0, -0.7071067812]); + assert_approx(quat_xyz([0,-90,0]), [0, -0.7071067812, 0, 0.7071067812]); + assert_approx(quat_xyz([0,360,0]), [0,0,0,-1]); + + assert_approx(quat_xyz([0,0,0]), [0,0,0,1]); + assert_approx(quat_xyz([0,0,30]), [0, 0, 0.2588190451, 0.9659258263]); + assert_approx(quat_xyz([0,0,90]), [0, 0, 0.7071067812, 0.7071067812]); + assert_approx(quat_xyz([0,0,-270]), [0, 0, -0.7071067812, -0.7071067812]); + assert_approx(quat_xyz([0,0,180]), [0,0,1,0]); + assert_approx(quat_xyz([0,0,270]), [0, 0, 0.7071067812, -0.7071067812]); + assert_approx(quat_xyz([0,0,-90]), [0, 0, -0.7071067812, 0.7071067812]); + assert_approx(quat_xyz([0,0,360]), [0,0,0,-1]); + + assert_approx(quat_xyz([30,30,30]), [0.1767766953, 0.3061862178, 0.1767766953, 0.9185586535]); + assert_approx(quat_xyz([12,34,56]), [-0.04824789229, 0.3036636044, 0.4195145429, 0.8540890495]); } -test_QuatZ(); +test_quat_xyz(); -module test_QuatXYZ() { - verify_f(QuatXYZ([0,0,0]), [0,0,0,1]); - verify_f(QuatXYZ([30,0,0]), [0.2588190451, 0, 0, 0.9659258263]); - verify_f(QuatXYZ([90,0,0]), [0.7071067812, 0, 0, 0.7071067812]); - verify_f(QuatXYZ([-270,0,0]), [-0.7071067812, 0, 0, -0.7071067812]); - verify_f(QuatXYZ([180,0,0]), [1,0,0,0]); - verify_f(QuatXYZ([270,0,0]), [0.7071067812, 0, 0, -0.7071067812]); - verify_f(QuatXYZ([-90,0,0]), [-0.7071067812, 0, 0, 0.7071067812]); - verify_f(QuatXYZ([360,0,0]), [0,0,0,-1]); - - verify_f(QuatXYZ([0,0,0]), [0,0,0,1]); - verify_f(QuatXYZ([0,30,0]), [0, 0.2588190451, 0, 0.9659258263]); - verify_f(QuatXYZ([0,90,0]), [0, 0.7071067812, 0, 0.7071067812]); - verify_f(QuatXYZ([0,-270,0]), [0, -0.7071067812, 0, -0.7071067812]); - verify_f(QuatXYZ([0,180,0]), [0,1,0,0]); - verify_f(QuatXYZ([0,270,0]), [0, 0.7071067812, 0, -0.7071067812]); - verify_f(QuatXYZ([0,-90,0]), [0, -0.7071067812, 0, 0.7071067812]); - verify_f(QuatXYZ([0,360,0]), [0,0,0,-1]); - - verify_f(QuatXYZ([0,0,0]), [0,0,0,1]); - verify_f(QuatXYZ([0,0,30]), [0, 0, 0.2588190451, 0.9659258263]); - verify_f(QuatXYZ([0,0,90]), [0, 0, 0.7071067812, 0.7071067812]); - verify_f(QuatXYZ([0,0,-270]), [0, 0, -0.7071067812, -0.7071067812]); - verify_f(QuatXYZ([0,0,180]), [0,0,1,0]); - verify_f(QuatXYZ([0,0,270]), [0, 0, 0.7071067812, -0.7071067812]); - verify_f(QuatXYZ([0,0,-90]), [0, 0, -0.7071067812, 0.7071067812]); - verify_f(QuatXYZ([0,0,360]), [0,0,0,-1]); - - verify_f(QuatXYZ([30,30,30]), [0.1767766953, 0.3061862178, 0.1767766953, 0.9185586535]); - verify_f(QuatXYZ([12,34,56]), [-0.04824789229, 0.3036636044, 0.4195145429, 0.8540890495]); +module test_q_from_to() { + assert_approx(q_mul(q_from_to([1,2,3], [4,5,2]),q_from_to([4,5,2], [1,2,3])), q_ident()); + assert_approx(q_matrix4(q_from_to([1,2,3], [4,5,2])), rot(from=[1,2,3],to=[4,5,2])); + assert_approx(q_rot(q_from_to([1,2,3], -[1,2,3]),[1,2,3]), -[1,2,3]); + assert_approx(unit(q_rot(q_from_to([1,2,3], [4,5,2]),[1,2,3])), unit([4,5,2])); } -test_QuatXYZ(); +test_q_from_to(); -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])); +module test_q_ident() { + assert_approx(q_ident(), [0,0,0,1]); } -test_Q_From_to(); +test_q_ident(); -module test_Q_Ident() { - verify_f(Q_Ident(), [0,0,0,1]); +module test_q_add_s() { + assert_approx(q_add_s([0,0,0,1],3),[0,0,0,4]); + assert_approx(q_add_s([0,0,1,0],3),[0,0,1,3]); + assert_approx(q_add_s([0,1,0,0],3),[0,1,0,3]); + assert_approx(q_add_s([1,0,0,0],3),[1,0,0,3]); + assert_approx(q_add_s(quat(LEFT+FWD,23),1),[-0.1409744184, -0.1409744184, 0, 1.979924705]); } -test_Q_Ident(); +test_q_add_s(); -module test_Q_Add_S() { - verify_f(Q_Add_S([0,0,0,1],3),[0,0,0,4]); - verify_f(Q_Add_S([0,0,1,0],3),[0,0,1,3]); - verify_f(Q_Add_S([0,1,0,0],3),[0,1,0,3]); - verify_f(Q_Add_S([1,0,0,0],3),[1,0,0,3]); - verify_f(Q_Add_S(Quat(LEFT+FWD,23),1),[-0.1409744184, -0.1409744184, 0, 1.979924705]); +module test_q_sub_s() { + assert_approx(q_sub_s([0,0,0,1],3),[0,0,0,-2]); + assert_approx(q_sub_s([0,0,1,0],3),[0,0,1,-3]); + assert_approx(q_sub_s([0,1,0,0],3),[0,1,0,-3]); + assert_approx(q_sub_s([1,0,0,0],3),[1,0,0,-3]); + assert_approx(q_sub_s(quat(LEFT+FWD,23),1),[-0.1409744184, -0.1409744184, 0, -0.02007529538]); } -test_Q_Add_S(); +test_q_sub_s(); -module test_Q_Sub_S() { - verify_f(Q_Sub_S([0,0,0,1],3),[0,0,0,-2]); - verify_f(Q_Sub_S([0,0,1,0],3),[0,0,1,-3]); - verify_f(Q_Sub_S([0,1,0,0],3),[0,1,0,-3]); - verify_f(Q_Sub_S([1,0,0,0],3),[1,0,0,-3]); - verify_f(Q_Sub_S(Quat(LEFT+FWD,23),1),[-0.1409744184, -0.1409744184, 0, -0.02007529538]); +module test_q_mul_s() { + assert_approx(q_mul_s([0,0,0,1],3),[0,0,0,3]); + assert_approx(q_mul_s([0,0,1,0],3),[0,0,3,0]); + assert_approx(q_mul_s([0,1,0,0],3),[0,3,0,0]); + assert_approx(q_mul_s([1,0,0,0],3),[3,0,0,0]); + assert_approx(q_mul_s([1,0,0,1],3),[3,0,0,3]); + assert_approx(q_mul_s(quat(LEFT+FWD,23),4),[-0.5638976735, -0.5638976735, 0, 3.919698818]); } -test_Q_Sub_S(); +test_q_mul_s(); -module test_Q_Mul_S() { - verify_f(Q_Mul_S([0,0,0,1],3),[0,0,0,3]); - verify_f(Q_Mul_S([0,0,1,0],3),[0,0,3,0]); - verify_f(Q_Mul_S([0,1,0,0],3),[0,3,0,0]); - verify_f(Q_Mul_S([1,0,0,0],3),[3,0,0,0]); - verify_f(Q_Mul_S([1,0,0,1],3),[3,0,0,3]); - verify_f(Q_Mul_S(Quat(LEFT+FWD,23),4),[-0.5638976735, -0.5638976735, 0, 3.919698818]); + +module test_q_div_s() { + assert_approx(q_div_s([0,0,0,1],3),[0,0,0,1/3]); + assert_approx(q_div_s([0,0,1,0],3),[0,0,1/3,0]); + assert_approx(q_div_s([0,1,0,0],3),[0,1/3,0,0]); + assert_approx(q_div_s([1,0,0,0],3),[1/3,0,0,0]); + assert_approx(q_div_s([1,0,0,1],3),[1/3,0,0,1/3]); + assert_approx(q_div_s(quat(LEFT+FWD,23),4),[-0.03524360459, -0.03524360459, 0, 0.2449811762]); } -test_Q_Mul_S(); +test_q_div_s(); - -module test_Q_Div_S() { - verify_f(Q_Div_S([0,0,0,1],3),[0,0,0,1/3]); - verify_f(Q_Div_S([0,0,1,0],3),[0,0,1/3,0]); - verify_f(Q_Div_S([0,1,0,0],3),[0,1/3,0,0]); - verify_f(Q_Div_S([1,0,0,0],3),[1/3,0,0,0]); - verify_f(Q_Div_S([1,0,0,1],3),[1/3,0,0,1/3]); - verify_f(Q_Div_S(Quat(LEFT+FWD,23),4),[-0.03524360459, -0.03524360459, 0, 0.2449811762]); +module test_q_add() { + assert_approx(q_add([2,3,4,5],[-1,-1,-1,-1]),[1,2,3,4]); + assert_approx(q_add([2,3,4,5],[-3,-3,-3,-3]),[-1,0,1,2]); + assert_approx(q_add([2,3,4,5],[0,0,0,0]),[2,3,4,5]); + assert_approx(q_add([2,3,4,5],[1,1,1,1]),[3,4,5,6]); + assert_approx(q_add([2,3,4,5],[1,0,0,0]),[3,3,4,5]); + assert_approx(q_add([2,3,4,5],[0,1,0,0]),[2,4,4,5]); + assert_approx(q_add([2,3,4,5],[0,0,1,0]),[2,3,5,5]); + assert_approx(q_add([2,3,4,5],[0,0,0,1]),[2,3,4,6]); + assert_approx(q_add([2,3,4,5],[2,1,2,1]),[4,4,6,6]); + assert_approx(q_add([2,3,4,5],[1,2,1,2]),[3,5,5,7]); } -test_Q_Div_S(); +test_q_add(); -module test_Q_Add() { - verify_f(Q_Add([2,3,4,5],[-1,-1,-1,-1]),[1,2,3,4]); - verify_f(Q_Add([2,3,4,5],[-3,-3,-3,-3]),[-1,0,1,2]); - verify_f(Q_Add([2,3,4,5],[0,0,0,0]),[2,3,4,5]); - verify_f(Q_Add([2,3,4,5],[1,1,1,1]),[3,4,5,6]); - verify_f(Q_Add([2,3,4,5],[1,0,0,0]),[3,3,4,5]); - verify_f(Q_Add([2,3,4,5],[0,1,0,0]),[2,4,4,5]); - verify_f(Q_Add([2,3,4,5],[0,0,1,0]),[2,3,5,5]); - verify_f(Q_Add([2,3,4,5],[0,0,0,1]),[2,3,4,6]); - verify_f(Q_Add([2,3,4,5],[2,1,2,1]),[4,4,6,6]); - verify_f(Q_Add([2,3,4,5],[1,2,1,2]),[3,5,5,7]); +module test_q_sub() { + assert_approx(q_sub([2,3,4,5],[-1,-1,-1,-1]),[3,4,5,6]); + assert_approx(q_sub([2,3,4,5],[-3,-3,-3,-3]),[5,6,7,8]); + assert_approx(q_sub([2,3,4,5],[0,0,0,0]),[2,3,4,5]); + assert_approx(q_sub([2,3,4,5],[1,1,1,1]),[1,2,3,4]); + assert_approx(q_sub([2,3,4,5],[1,0,0,0]),[1,3,4,5]); + assert_approx(q_sub([2,3,4,5],[0,1,0,0]),[2,2,4,5]); + assert_approx(q_sub([2,3,4,5],[0,0,1,0]),[2,3,3,5]); + assert_approx(q_sub([2,3,4,5],[0,0,0,1]),[2,3,4,4]); + assert_approx(q_sub([2,3,4,5],[2,1,2,1]),[0,2,2,4]); + assert_approx(q_sub([2,3,4,5],[1,2,1,2]),[1,1,3,3]); } -test_Q_Add(); +test_q_sub(); -module test_Q_Sub() { - verify_f(Q_Sub([2,3,4,5],[-1,-1,-1,-1]),[3,4,5,6]); - verify_f(Q_Sub([2,3,4,5],[-3,-3,-3,-3]),[5,6,7,8]); - verify_f(Q_Sub([2,3,4,5],[0,0,0,0]),[2,3,4,5]); - verify_f(Q_Sub([2,3,4,5],[1,1,1,1]),[1,2,3,4]); - verify_f(Q_Sub([2,3,4,5],[1,0,0,0]),[1,3,4,5]); - verify_f(Q_Sub([2,3,4,5],[0,1,0,0]),[2,2,4,5]); - verify_f(Q_Sub([2,3,4,5],[0,0,1,0]),[2,3,3,5]); - verify_f(Q_Sub([2,3,4,5],[0,0,0,1]),[2,3,4,4]); - verify_f(Q_Sub([2,3,4,5],[2,1,2,1]),[0,2,2,4]); - verify_f(Q_Sub([2,3,4,5],[1,2,1,2]),[1,1,3,3]); +module test_q_mul() { + assert_approx(q_mul(quat_z(30),quat_x(57)),[0.4608999698, 0.1234977747, 0.2274546059, 0.8488721457]); + assert_approx(q_mul(quat_y(30),quat_z(23)),[0.05160021841, 0.2536231763, 0.1925746368, 0.94653458]); } -test_Q_Sub(); +test_q_mul(); -module test_Q_Mul() { - verify_f(Q_Mul(QuatZ(30),QuatX(57)),[0.4608999698, 0.1234977747, 0.2274546059, 0.8488721457]); - verify_f(Q_Mul(QuatY(30),QuatZ(23)),[0.05160021841, 0.2536231763, 0.1925746368, 0.94653458]); +module test_q_cumulative() { + assert_approx(q_cumulative([quat_z(30),quat_x(57),quat_y(18)]),[[0, 0, 0.2588190451, 0.9659258263], [0.4608999698, -0.1234977747, 0.2274546059, 0.8488721457], [0.4908072659, 0.01081554785, 0.1525536221, 0.8577404293]]); } -test_Q_Mul(); +test_q_cumulative(); -module test_Q_Cumulative() { - verify_f(Q_Cumulative([QuatZ(30),QuatX(57),QuatY(18)]),[[0, 0, 0.2588190451, 0.9659258263], [0.4608999698, -0.1234977747, 0.2274546059, 0.8488721457], [0.4908072659, 0.01081554785, 0.1525536221, 0.8577404293]]); +module test_q_dot() { + assert_approx(q_dot(quat_z(30),quat_x(57)),0.8488721457); + assert_approx(q_dot(quat_y(30),quat_z(23)),0.94653458); } -test_Q_Cumulative(); +test_q_dot(); -module test_Q_Dot() { - verify_f(Q_Dot(QuatZ(30),QuatX(57)),0.8488721457); - verify_f(Q_Dot(QuatY(30),QuatZ(23)),0.94653458); +module test_q_neg() { + assert_approx(q_neg([1,0,0,1]),[-1,0,0,-1]); + assert_approx(q_neg([0,1,1,0]),[0,-1,-1,0]); + assert_approx(q_neg(quat_xyz([23,45,67])),[0.0533818345,-0.4143703268,-0.4360652669,-0.7970537592]); } -test_Q_Dot(); +test_q_neg(); -module test_Q_Neg() { - verify_f(Q_Neg([1,0,0,1]),[-1,0,0,-1]); - verify_f(Q_Neg([0,1,1,0]),[0,-1,-1,0]); - verify_f(Q_Neg(QuatXYZ([23,45,67])),[0.0533818345,-0.4143703268,-0.4360652669,-0.7970537592]); +module test_q_conj() { + assert_approx(q_conj([1,0,0,1]),[-1,0,0,1]); + assert_approx(q_conj([0,1,1,0]),[0,-1,-1,0]); + assert_approx(q_conj(quat_xyz([23,45,67])),[0.0533818345, -0.4143703268, -0.4360652669, 0.7970537592]); } -test_Q_Neg(); +test_q_conj(); -module test_Q_Conj() { - verify_f(Q_Conj([1,0,0,1]),[-1,0,0,1]); - verify_f(Q_Conj([0,1,1,0]),[0,-1,-1,0]); - verify_f(Q_Conj(QuatXYZ([23,45,67])),[0.0533818345, -0.4143703268, -0.4360652669, 0.7970537592]); +module test_q_inverse() { + + assert_approx(q_inverse([1,0,0,1]),[-1,0,0,1]/sqrt(2)); + assert_approx(q_inverse([0,1,1,0]),[0,-1,-1,0]/sqrt(2)); + assert_approx(q_inverse(quat_xyz([23,45,67])),q_conj(quat_xyz([23,45,67]))); + assert_approx(q_mul(q_inverse(quat_xyz([23,45,67])),quat_xyz([23,45,67])),q_ident()); } -test_Q_Conj(); +test_q_inverse(); -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()); +module test_q_Norm() { + assert_approx(q_norm([1,0,0,1]),1.414213562); + assert_approx(q_norm([0,1,1,0]),1.414213562); + assert_approx(q_norm(quat_xyz([23,45,67])),1); } -test_Q_Inverse(); +test_q_Norm(); -module test_Q_Norm() { - verify_f(Q_Norm([1,0,0,1]),1.414213562); - verify_f(Q_Norm([0,1,1,0]),1.414213562); - verify_f(Q_Norm(QuatXYZ([23,45,67])),1); +module test_q_normalize() { + assert_approx(q_normalize([1,0,0,1]),[0.7071067812, 0, 0, 0.7071067812]); + assert_approx(q_normalize([0,1,1,0]),[0, 0.7071067812, 0.7071067812, 0]); + assert_approx(q_normalize(quat_xyz([23,45,67])),[-0.0533818345, 0.4143703268, 0.4360652669, 0.7970537592]); } -test_Q_Norm(); +test_q_normalize(); -module test_Q_Normalize() { - verify_f(Q_Normalize([1,0,0,1]),[0.7071067812, 0, 0, 0.7071067812]); - verify_f(Q_Normalize([0,1,1,0]),[0, 0.7071067812, 0.7071067812, 0]); - verify_f(Q_Normalize(QuatXYZ([23,45,67])),[-0.0533818345, 0.4143703268, 0.4360652669, 0.7970537592]); +module test_q_dist() { + assert_approx(q_dist(quat_xyz([23,45,67]),quat_xyz([23,45,67])),0); + assert_approx(q_dist(quat_xyz([23,45,67]),quat_xyz([12,34,56])),0.1257349854); } -test_Q_Normalize(); +test_q_dist(); -module test_Q_Dist() { - verify_f(Q_Dist(QuatXYZ([23,45,67]),QuatXYZ([23,45,67])),0); - verify_f(Q_Dist(QuatXYZ([23,45,67]),QuatXYZ([12,34,56])),0.1257349854); +module test_q_slerp() { + assert_approx(q_slerp(quat_x(45),quat_y(30),0.0),quat_x(45)); + assert_approx(q_slerp(quat_x(45),quat_y(30),0.5),[0.1967063121, 0.1330377423, 0, 0.9713946602]); + assert_approx(q_slerp(quat_x(45),quat_y(30),1.0),quat_y(30)); } -test_Q_Dist(); +test_q_slerp(); -module test_Q_Slerp() { - verify_f(Q_Slerp(QuatX(45),QuatY(30),0.0),QuatX(45)); - verify_f(Q_Slerp(QuatX(45),QuatY(30),0.5),[0.1967063121, 0.1330377423, 0, 0.9713946602]); - verify_f(Q_Slerp(QuatX(45),QuatY(30),1.0),QuatY(30)); +module test_q_matrix3() { + assert_approx(q_matrix3(quat_z(37)),rot(37,planar=true)); + assert_approx(q_matrix3(quat_z(-49)),rot(-49,planar=true)); } -test_Q_Slerp(); +test_q_matrix3(); -module test_Q_Matrix3() { - verify_f(Q_Matrix3(QuatZ(37)),rot(37,planar=true)); - verify_f(Q_Matrix3(QuatZ(-49)),rot(-49,planar=true)); +module test_q_matrix4() { + assert_approx(q_matrix4(quat_z(37)),rot(37)); + assert_approx(q_matrix4(quat_z(-49)),rot(-49)); + assert_approx(q_matrix4(quat_x(37)),rot([37,0,0])); + assert_approx(q_matrix4(quat_y(37)),rot([0,37,0])); + assert_approx(q_matrix4(quat_xyz([12,34,56])),rot([12,34,56])); } -test_Q_Matrix3(); +test_q_matrix4(); -module test_Q_Matrix4() { - verify_f(Q_Matrix4(QuatZ(37)),rot(37)); - verify_f(Q_Matrix4(QuatZ(-49)),rot(-49)); - verify_f(Q_Matrix4(QuatX(37)),rot([37,0,0])); - verify_f(Q_Matrix4(QuatY(37)),rot([0,37,0])); - verify_f(Q_Matrix4(QuatXYZ([12,34,56])),rot([12,34,56])); +module test_q_axis() { + assert_approx(q_axis(quat_x(37)),RIGHT); + assert_approx(q_axis(quat_x(-37)),LEFT); + assert_approx(q_axis(quat_y(37)),BACK); + assert_approx(q_axis(quat_y(-37)),FWD); + assert_approx(q_axis(quat_z(37)),UP); + assert_approx(q_axis(quat_z(-37)),DOWN); } -test_Q_Matrix4(); +test_q_axis(); -module test_Q_Axis() { - verify_f(Q_Axis(QuatX(37)),RIGHT); - verify_f(Q_Axis(QuatX(-37)),LEFT); - verify_f(Q_Axis(QuatY(37)),BACK); - verify_f(Q_Axis(QuatY(-37)),FWD); - verify_f(Q_Axis(QuatZ(37)),UP); - verify_f(Q_Axis(QuatZ(-37)),DOWN); +module test_q_angle() { + assert_approx(q_angle(quat_x(0)),0); + assert_approx(q_angle(quat_y(0)),0); + assert_approx(q_angle(quat_z(0)),0); + assert_approx(q_angle(quat_x(37)),37); + assert_approx(q_angle(quat_x(-37)),37); + assert_approx(q_angle(quat_y(37)),37); + assert_approx(q_angle(quat_y(-37)),37); + assert_approx(q_angle(quat_z(37)),37); + assert_approx(q_angle(quat_z(-37)),37); + + assert_approx(q_angle(quat_z(-37),quat_z(-37)), 0); + assert_approx(q_angle(quat_z( 37.123),quat_z(-37.123)), 74.246); + assert_approx(q_angle(quat_x( 37),quat_y(-37)), 51.86293283); } -test_Q_Axis(); +test_q_angle(); -module test_Q_Angle() { - verify_f(Q_Angle(QuatX(0)),0); - verify_f(Q_Angle(QuatY(0)),0); - verify_f(Q_Angle(QuatZ(0)),0); - verify_f(Q_Angle(QuatX(37)),37); - verify_f(Q_Angle(QuatX(-37)),37); - verify_f(Q_Angle(QuatY(37)),37); - 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); +module test_q_rot() { + assert_approx(q_rot(quat_xyz([12,34,56])),rot([12,34,56])); + assert_approx(q_rot(quat_xyz([12,34,56]),p=[2,3,4]),rot([12,34,56],p=[2,3,4])); + assert_approx(q_rot(quat_xyz([12,34,56]),p=[[2,3,4],[4,9,6]]),rot([12,34,56],p=[[2,3,4],[4,9,6]])); } -test_Q_Angle(); +test_q_rot(); -module test_Qrot() { - verify_f(Qrot(QuatXYZ([12,34,56])),rot([12,34,56])); - verify_f(Qrot(QuatXYZ([12,34,56]),p=[2,3,4]),rot([12,34,56],p=[2,3,4])); - verify_f(Qrot(QuatXYZ([12,34,56]),p=[[2,3,4],[4,9,6]]),rot([12,34,56],p=[[2,3,4],[4,9,6]])); +module test_q_rotation() { + assert_approx(_q_standard(q_rotation(q_matrix3(quat([12,34,56],33)))),_q_standard(quat([12,34,56],33))); + assert_approx(q_matrix3(q_rotation(q_matrix3(quat_xyz([12,34,56])))), + q_matrix3(quat_xyz([12,34,56]))); } -test_Qrot(); +test_q_rotation(); -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() { + assert_approx(q_rotation_path(quat_x(135), 5, quat_y(13.5))[0] , q_matrix4(quat_x(135))); + assert_approx(q_rotation_path(quat_x(135), 11, quat_y(13.5))[11] , yrot(13.5)); + assert_approx(q_rotation_path(quat_x(135), 16, quat_y(13.5))[8] , q_rotation_path(quat_x(135), 8, quat_y(13.5))[4]); + assert_approx(q_rotation_path(quat_x(135), 16, quat_y(13.5))[7] , + q_rotation_path(quat_y(13.5),16, quat_x(135))[9]); - -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)); + assert_approx(q_rotation_path(quat_x(11), 5)[0] , xrot(11)); + assert_approx(q_rotation_path(quat_x(11), 5)[4] , xrot(55)); } -test_Q_Rotation_path(); +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)); +module test_q_nlerp() { + assert_approx(q_nlerp(quat_x(45),quat_y(30),0.0),quat_x(45)); + assert_approx(q_nlerp(quat_x(45),quat_y(30),0.5),[0.1967063121, 0.1330377423, 0, 0.9713946602]); + assert_approx(q_rotation_path(quat_x(135), 16, quat_y(13.5))[8] , q_matrix4(q_nlerp(quat_x(135), quat_y(13.5),0.5))); + assert_approx(q_nlerp(quat_x(45),quat_y(30),1.0),quat_y(30)); } -test_Q_Nlerp(); +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)); +module test_q_squad() { + assert_approx(q_squad(quat_x(45),quat_z(30),quat_x(90),quat_y(30),0.0),quat_x(45)); + assert_approx(q_squad(quat_x(45),quat_z(30),quat_x(90),quat_y(30),1.0),quat_y(30)); + assert_approx(q_squad(quat_x(0),quat_x(30),quat_x(90),quat_x(120),0.5), + q_slerp(quat_x(0),quat_x(120),0.5)); + assert_approx(q_squad(quat_y(0),quat_y(0),quat_x(120),quat_x(120),0.3), + q_slerp(quat_y(0),quat_x(120),0.3)); } -test_Q_Squad(); +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)))); +module test_q_exp() { + assert_approx(q_exp(q_ident()), exp(1)*q_ident()); + assert_approx(q_exp([0,0,0,33.7]), exp(33.7)*q_ident()); + assert_approx(q_exp(q_ln(q_ident())), q_ident()); + assert_approx(q_exp(q_ln([1,2,3,0])), [1,2,3,0]); + assert_approx(q_exp(q_ln(quat_xyz([31,27,34]))), quat_xyz([31,27,34])); + let(q=quat_xyz([12,23,34])) + assert_approx(q_exp(q+q_inverse(q)),q_mul(q_exp(q),q_exp(q_inverse(q)))); } -test_Q_exp(); +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]); +module test_q_ln() { + assert_approx(q_ln([1,2,3,0]), [24.0535117721, 48.1070235442, 72.1605353164, 1.31952866481]); + assert_approx(q_ln(q_ident()), [0,0,0,0]); + assert_approx(q_ln(5.5*q_ident()), [0,0,0,ln(5.5)]); + assert_approx(q_ln(q_exp(quat_xyz([13,37,43]))), quat_xyz([13,37,43])); + assert_approx(q_ln(quat_xyz([12,23,34]))+q_ln(q_inverse(quat_xyz([12,23,34]))), [0,0,0,0]); } -test_Q_ln(); +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); +module test_q_pow() { + q = quat([1,2,3],77); + assert_approx(q_pow(q,1), q); + assert_approx(q_pow(q,0), q_ident()); + assert_approx(q_pow(q,-1), q_inverse(q)); + assert_approx(q_pow(q,2), q_mul(q,q)); + assert_approx(q_pow(q,3), q_mul(q,q_pow(q,2))); + assert_approx(q_mul(q_pow(q,0.456),q_pow(q,0.544)), q); + assert_approx(q_mul(q_pow(q,0.335),q_mul(q_pow(q,.552),q_pow(q,.113))), q); } -test_Q_pow(); +test_q_pow();