From 8e8f22ae25139e1878f3bd66b9c3769785fae817 Mon Sep 17 00:00:00 2001 From: Garth Minette Date: Mon, 7 Mar 2022 18:42:40 -0800 Subject: [PATCH 1/3] Removed quaternions.scad --- quaternions.scad | 661 ----------------------------------------------- std.scad | 1 - 2 files changed, 662 deletions(-) delete mode 100644 quaternions.scad diff --git a/quaternions.scad b/quaternions.scad deleted file mode 100644 index 88cd84f..0000000 --- a/quaternions.scad +++ /dev/null @@ -1,661 +0,0 @@ -/////////////////////////////////////////// -// LibFile: quaternions.scad -// Support for Quaternions. -// Includes: -// include -// FileGroup: Math -// FileSummary: Quaternion based rotations that avoid gimbal lock issues. -// FileFootnotes: STD=Included in std.scad -/////////////////////////////////////////// - - -// Section: Quaternions -// Quaternions are fast methods of storing and calculating arbitrary rotations. -// Quaternions contain information on both axis of rotation, and rotation angle. -// You can chain multiple rotation together by multiplying quaternions together. -// They don't suffer from the gimbal-lock issues that `[X,Y,Z]` rotation angles do. -// Quaternions are stored internally as a 4-value vector: -// `[X,Y,Z,W]`, where the quaternion formula is `W+Xi+Yj+Zk` - - -// 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: is_quaternion() -// Usage: -// if(is_quaternion(q)) a=0; -// Description: Return true if q is a valid non-zero quaternion. -// Arguments: -// q = object to check. -function is_quaternion(q) = is_vector(q,4) && ! approx(norm(q),0) ; - - -// Function: quat() -// Usage: -// 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) = - 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: quat_x() -// Usage: -// 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 quat_x(a=0) = - assert( is_finite(a), "Invalid angle" ) - quat([1,0,0],a); - - -// Function: quat_y() -// Usage: -// 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 quat_y(a=0) = - assert( is_finite(a), "Invalid angle" ) - quat([0,1,0],a); - - -// Function: quat_z() -// Usage: -// 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 quat_z(a=0) = - assert( is_finite(a), "Invalid angle" ) - quat([0,0,1],a); - - -// Function: quat_xyz() -// Usage: -// 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 quat_xyz(a=[0,0,0]) = - assert( is_vector(a,3), "Invalid angles") - let( - qx = quat_x(a[0]), - qy = quat_y(a[1]), - qz = quat_z(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]; - - -// Function: q_add_s() -// Usage: -// 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) = - 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`. -// 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`. -// 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`. -// 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. -// The returned quaternion is usually not normalized. -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() -// Usage: -// 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( is_quaternion(a) && is_quaternion(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`. -// The returned quaternion is normalized if both `a` and `b` are normalized -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, - 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() -// Usage: -// 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=[]) = - _i==len(v) ? _acc : - q_cumulative( - v, _i+1, - concat( - _acc, - [_i==0 ? v[_i] : q_mul(v[_i], last(_acc))] - ) - ); - - -// Function: q_dot() -// Usage: -// q_dot(a, b) -// Description: Calculates the dot product between quaternions `a` and `b`. -function q_dot(a, b) = - assert( is_quaternion(a) && is_quaternion(b), "Invalid quaternion(s)" ) - a*b; - -// Function: q_neg() -// Usage: -// q_neg(q) -// Description: Returns the negative of quaternion `q`. -function q_neg(q) = - assert( is_quaternion(q), "Invalid quaternion" ) - -q; - - -// Function: q_conj() -// Usage: -// q_conj(q) -// Description: Returns the conjugate of quaternion `q`. -function q_conj(q) = - assert( is_quaternion(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( is_quaternion(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`. -// Normalized quaternions have unitary norm. -function q_norm(q) = - assert( is_quaternion(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) = - assert( is_quaternion(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) = - assert( is_quaternion(q1) && is_quaternion(q2), "Invalid quaternion(s)" ) - norm(q2-q1); - - -// Function: q_slerp() -// Usage: -// q_slerp(q1, q2, u); -// Description: -// Returns a quaternion that is a spherical interpolation between two quaternions. -// Arguments: -// q1 = The first quaternion. (u=0) -// 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 = quat_y(-135); -// b = quat_xyz([0,-30,30]); -// for (u=[0:0.1:1]) -// q_rot(q_slerp(a, b, u)) -// right(80) cube([10,10,1]); -// #sphere(r=80); -// Example(3D): Giving `u` as a Range -// 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) = - is_undef(_dot) - ? assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(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) - : _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() -// Usage: -// q_matrix3(q); -// Description: -// Returns the 3x3 rotation matrix for the given normalized quaternion 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]], - [ 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() -// Usage: -// q_matrix4(q); -// Description: -// Returns the 4x4 rotation matrix for the given normalized quaternion 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], - [ 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() -// Usage: -// 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( is_quaternion(q) , "Invalid quaternion" ) - let( d = norm(_qvec(q)) ) - approx(d,0)? [0,0,1] : _qvec(q)/d; - -// Function: q_angle() -// Usage: -// 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(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)) - : let( q1 = q1/norm(q1), - q2 = q2/norm(q2) ) - 4 * atan2(norm(q1 - q2), norm(q1 + q2)); - -// Function&Module: q_rot() -// Usage: As Module -// q_rot(q) ... -// Usage: As Function -// 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 = quat_xyz([90,-15,-45]); -// q_rot(q) shape(); -// #shape(); -// Example(NORENDER): -// q = quat_xyz([45,35,10]); -// mat4x4 = q_rot(q); -// Example(NORENDER): -// q = quat_xyz([45,35,10]); -// pt = q_rot(q, p=[4,5,6]); -// Example(NORENDER): -// 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 q_rot(q,p) = - is_undef(p)? q_matrix4(q) : - is_vector(p)? q_rot(q,[p])[0] : - apply(q_matrix4(q), p); - - -// Module: q_rot_copies() -// Usage: -// 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) q_rot(q) ...`. -// Arguments: -// quats = A list containing all quaternions to rotate to and create copies of all children for. -// Example: -// 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 q_rot_copies(quats) for (q=quats) q_rot(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() -// 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 = 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 = 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 = 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 = 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( 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]; - -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 = quat_y(-135); -// b = quat_xyz([0,-30,30]); -// for (u=[0:0.1:1]) -// q_rot(q_nlerp(a, b, u)) -// right(80) cube([10,10,1]); -// #sphere(r=80); -// Example(3D): Giving `u` as a Range -// 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) = - assert(is_finite(u) || is_range(u) || is_vector(u) , - "Invalid interpolation coefficient(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) ) - 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 = quat_y(-135); -// b = quat_xyz([-50,-50,120]); -// c = quat_xyz([-50,-40,30]); -// d = quat_y(-45); -// color("red"){ -// q_rot(b) right(80) cube([10,10,1]); -// q_rot(c) right(80) cube([10,10,1]); -// } -// for (u=[0:0.05:1]) -// 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 = 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) = - 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(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() -// 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( 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)); - - - -// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap diff --git a/std.scad b/std.scad index ccfa7b6..ffdd7e1 100644 --- a/std.scad +++ b/std.scad @@ -27,7 +27,6 @@ include include include include -include include include include From 9e544a8489ac210ed5cf0633e4a85dacbed04f1d Mon Sep 17 00:00:00 2001 From: Garth Minette Date: Mon, 7 Mar 2022 18:59:05 -0800 Subject: [PATCH 2/3] Added atype= to rect() --- .openscad_docsgen_rc | 1 + shapes2d.scad | 33 +++++++++++++++++++++++++++------ 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/.openscad_docsgen_rc b/.openscad_docsgen_rc index 8693302..5facf70 100644 --- a/.openscad_docsgen_rc +++ b/.openscad_docsgen_rc @@ -50,6 +50,7 @@ PrioritizeFiles: screw_drive.scad DefineHeader(BulletList): Side Effects DefineHeader(Table;Headers=Anchor Name|Position): Extra Anchors +DefineHeader(Table;Headers=Anchor Type|What it is): Anchor Types DefineHeader(Table;Headers=Name|Definition): Terminology DefineHeader(BulletList): Requirements diff --git a/shapes2d.scad b/shapes2d.scad index d523dfe..b2b8bda 100644 --- a/shapes2d.scad +++ b/shapes2d.scad @@ -84,10 +84,15 @@ module square(size=1, center, anchor, spin) { // When called as a function, returns a 2D path/list of points for a square/rectangle of the given size. // Arguments: // size = The size of the rectangle to create. If given as a scalar, both X and Y will be the same size. +// --- // rounding = The rounding radius for the corners. If negative, produces external roundover spikes on the X axis. If given as a list of four numbers, gives individual radii for each corner, in the order [X+Y+,X-Y+,X-Y-,X+Y-]. Default: 0 (no rounding) // chamfer = The chamfer size for the corners. If negative, produces external chamfer spikes on the X axis. If given as a list of four numbers, gives individual chamfers for each corner, in the order [X+Y+,X-Y+,X-Y-,X+Y-]. Default: 0 (no chamfer) +// atype = The type of anchoring to use with `anchor=`. Valid opptions are "box" and "perim". This lets you choose between putting anchors on the rounded or chamfered perimeter, or on the square bounding box of the shape. Default: "box" // anchor = Translate so anchor point is at origin (0,0,0). See [anchor](attachments.scad#subsection-anchor). Default: `CENTER` // spin = Rotate this many degrees around the Z axis after anchor. See [spin](attachments.scad#subsection-spin). Default: `0` +// Anchor Types: +// box = Anchor is with respect to the rectangular bounding box of the shape. +// perim = Anchors are placed along the rounded or chamfered perimeter of the shape. // Example(2D): // rect(40); // Example(2D): Anchored @@ -102,13 +107,21 @@ module square(size=1, center, anchor, spin) { // rect([40,30], chamfer=-5); // Example(2D): Negative-Rounded Rect // rect([40,30], rounding=-5); +// Example(2D): Default "box" Anchors +// color("red") rect([40,30]); +// rect([40,30], rounding=10) +// show_anchors(); +// Example(2D): "perim" Anchors +// rect([40,30], rounding=10, atype="perim") +// show_anchors(); // Example(2D): Mixed Chamferring and Rounding // rect([40,30],rounding=[5,0,10,0],chamfer=[0,8,0,15],$fa=1,$fs=1); // Example(2D): Called as Function // path = rect([40,30], chamfer=5, anchor=FRONT, spin=30); // stroke(path, closed=true); // move_copies(path) color("blue") circle(d=2,$fn=8); -module rect(size=1, rounding=0, chamfer=0, anchor=CENTER, spin=0) { +module rect(size=1, rounding=0, atype="box", chamfer=0, anchor=CENTER, spin=0) { + errchk = assert(in_list(atype, ["box", "perim"])); size = is_num(size)? [size,size] : point2d(size); if (rounding==0 && chamfer==0) { attachable(anchor, spin, two_d=true, size=size) { @@ -117,19 +130,27 @@ module rect(size=1, rounding=0, chamfer=0, anchor=CENTER, spin=0) { } } else { pts = rect(size=size, rounding=rounding, chamfer=chamfer); - attachable(anchor, spin, two_d=true, path=pts) { - polygon(pts); - children(); + if (atype == "perim") { + attachable(anchor, spin, two_d=true, path=pts) { + polygon(pts); + children(); + } + } else { + attachable(anchor, spin, two_d=true, size=size) { + polygon(pts); + children(); + } } } } -function rect(size=1, rounding=0, chamfer=0, anchor=CENTER, spin=0) = +function rect(size=1, rounding=0, chamfer=0, atype="box", anchor=CENTER, spin=0) = assert(is_num(size) || is_vector(size)) assert(is_num(chamfer) || len(chamfer)==4) assert(is_num(rounding) || len(rounding)==4) + assert(in_list(atype, ["box", "perim"])) let( anchor=point2d(anchor), size = is_num(size)? [size,size] : point2d(size), @@ -176,7 +197,7 @@ function rect(size=1, rounding=0, chamfer=0, anchor=CENTER, spin=0) = ) each move(cp, p=qrpts) ] - ) complex? + ) complex && atype=="perim"? reorient(anchor,spin, two_d=true, path=path, p=path) : reorient(anchor,spin, two_d=true, size=size, p=path); From af024c47c2b67ba9aa4eae0c4bba9e42c2b35b72 Mon Sep 17 00:00:00 2001 From: Garth Minette Date: Mon, 7 Mar 2022 22:49:59 -0800 Subject: [PATCH 3/3] Remove quaternion tests. --- tests/test_quaternions.scad | 388 ------------------------------------ 1 file changed, 388 deletions(-) delete mode 100644 tests/test_quaternions.scad diff --git a/tests/test_quaternions.scad b/tests/test_quaternions.scad deleted file mode 100644 index f0f0fe8..0000000 --- a/tests/test_quaternions.scad +++ /dev/null @@ -1,388 +0,0 @@ -include <../std.scad> - - - -function _q_standard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q; - - -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_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_quat(); - - -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_x(); - - -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_quat_y(); - - -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_quat_z(); - - -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_quat_xyz(); - - -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_q_from_to(); - - -module test_q_ident() { - assert_approx(q_ident(), [0,0,0,1]); -} -test_q_ident(); - - -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_add_s(); - - -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_sub_s(); - - -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_mul_s(); - - - -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_div_s(); - - -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_add(); - - -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_sub(); - - -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_mul(); - - -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_cumulative(); - - -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_dot(); - - -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_neg(); - - -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_conj(); - - -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_inverse(); - - -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_Norm(); - - -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_normalize(); - - -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_dist(); - - -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_slerp(); - - -module test_q_matrix3() { - assert_approx(q_matrix3(quat_z(37)),affine2d_zrot(37)); - assert_approx(q_matrix3(quat_z(-49)),affine2d_zrot(-49)); -} -test_q_matrix3(); - - -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_matrix4(); - - -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_axis(); - - -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_angle(); - - -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_rot(); - - -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_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]); - - 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(); - - -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(); - - -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(); - - -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(); - - -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(); - - -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(); - - - - - - -// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap