From 1ced82f16c80751bc1d49bdb6bc5ff425488e41d Mon Sep 17 00:00:00 2001 From: RonaldoCMP Date: Wed, 26 Aug 2020 14:07:12 +0100 Subject: [PATCH 1/5] Correction of C_times validation --- arrays.scad | 127 ++++++++++++++++++++--- math.scad | 2 +- quaternions.scad | 170 +++++++++++++++++-------------- tests/test_all.scad | 28 ++++++ tests/test_quaternions.scad | 196 ++++++++++++++++++++++++++++-------- 5 files changed, 391 insertions(+), 132 deletions(-) create mode 100644 tests/test_all.scad diff --git a/arrays.scad b/arrays.scad index 835fd65..3ae7a88 100644 --- a/arrays.scad +++ b/arrays.scad @@ -708,36 +708,108 @@ function _sort_vectors4(arr) = y ] ) concat( _sort_vectors4(lesser), equal, _sort_vectors4(greater) ); +// sort a list of vectors +function _sort_vectors(arr, _i=0) = + len(arr)<=1 || _i>=len(arr[0]) ? arr : + let( + pivot = arr[floor(len(arr)/2)][_i], + lesser = [ for (entry=arr) if (entry[_i] < pivot ) entry ], + equal = [ for (entry=arr) if (entry[_i] == pivot ) entry ], + greater = [ for (entry=arr) if (entry[_i] > pivot ) entry ] + ) + concat( + _sort_vectors(lesser, _i ), + _sort_vectors(equal, _i+1 ), + _sort_vectors(greater, _i ) ); + +// given pairs of an index and a vector, return the list of indices of the list sorted by the vectors +function _sort_vectors_indexed(arr, _i=0) = + arr==[] ? [] : + len(arr)==1 || _i>=len(arr[0][1]) ? [for(ai=arr) ai[0]] : + let( + pivot = arr[floor(len(arr)/2)][1][_i], + lesser = [ for (entry=arr) if (entry[1][_i] < pivot ) entry ], + equal = [ for (entry=arr) if (entry[1][_i] == pivot ) entry ], + greater = [ for (entry=arr) if (entry[1][_i] > pivot ) entry ] + ) + concat( + _sort_vectors_indexed(lesser, _i ), + _sort_vectors_indexed(equal, _i+1 ), + _sort_vectors_indexed(greater, _i ) ); + // when idx==undef, returns the sorted array // otherwise, returns the indices of the sorted array function _sort_general(arr, idx=undef) = - (len(arr)<=1) ? arr : + len(arr)<=1 ? arr : is_undef(idx) - ? _sort_scalar(arr) + ? _simple_sort(arr) +// : _lexical_sort(arr) : let( arrind=[for(k=[0:len(arr)-1], ark=[arr[k]]) [ k, [for (i=idx) ark[i]] ] ] ) - _indexed_sort(arrind); - -// given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair -// the sorting is done using compare_vals() -function _indexed_sort(arrind) = - arrind==[] ? [] : len(arrind)==1? [arrind[0][0]] : - let( pivot = arrind[floor(len(arrind)/2)][1] ) + _indexed_sort(arrind ); + +// sort simple lists with compare_vals() +function _simple_sort(arr) = + arr==[] || len(arr)==1 ? arr : let( - lesser = [ for (entry=arrind) if (compare_vals(entry[1], pivot) <0 ) entry ], - equal = [ for (entry=arrind) if (compare_vals(entry[1], pivot)==0 ) entry[0] ], - greater = [ for (entry=arrind) if (compare_vals(entry[1], pivot) >0 ) entry ] + pivot = arr[floor(len(arr)/2)], + lesser = [ for (entry=arr) if (compare_vals(entry, pivot) <0 ) entry ], + equal = [ for (entry=arr) if (compare_vals(entry, pivot)==0 ) entry ], + greater = [ for (entry=arr) if (compare_vals(entry, pivot) >0 ) entry ] ) - concat(_indexed_sort(lesser), equal, _indexed_sort(greater)); + concat( + _simple_sort(lesser), + equal, + _simple_sort(greater) + ); + + +// given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair +// it uses compare_vals() +function _lexical_sort(arr, _i=0) = + arr==[] || len(arr)==1 || _i>=len(arr[0]) ? arr : + let( + pivot = arr[floor(len(arr)/2)][_i], + lesser = [ for (entry=arr) if (compare_vals(entry[_i], pivot) <0 ) entry ], + equal = [ for (entry=arr) if (compare_vals(entry[_i], pivot)==0 ) entry ], + greater = [ for (entry=arr) if (compare_vals(entry[_i], pivot) >0 ) entry ] + ) + concat( + _lexical_sort(lesser, _i ), + _lexical_sort(equal, _i+1 ), + _lexical_sort(greater, _i ) + ); + + +// given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair +// it uses compare_vals() +function _indexed_sort(arr, _i=0) = + arr==[] ? [] : + len(arr)==1? [arr[0][0]] : + _i>=len(arr[0][1]) ? [for(ai=arr) ai[0]] : + let( + pivot = arr[floor(len(arr)/2)][1][_i], + lesser = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot) <0 ) entry ], + equal = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot)==0 ) entry ], + greater = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot) >0 ) entry ] + ) + concat( + _indexed_sort(lesser, _i ), + _indexed_sort(equal, _i+1 ), + _indexed_sort(greater, _i ) ); // returns true for valid index specifications idx in the interval [imin, imax) // note that idx can't have any value greater or EQUAL to imax +// this allows imax=INF as a bound to numerical lists function _valid_idx(idx,imin,imax) = is_undef(idx) || ( is_finite(idx) && idx>=imin && idx< imax ) || ( is_list(idx) && min(idx)>=imin && max(idx)< imax ) - || ( valid_range(idx) && idx[0]>=imin && idx[2]< imax ); + || ( ! is_list(idx) // it implicitly is a range + && (idx[1]>0 && idx[0]>=imin && idx[2]< imax) + || + (idx[0]=imin) ); // Function: sort() @@ -758,7 +830,7 @@ function _valid_idx(idx,imin,imax) = function sort(list, idx=undef) = !is_list(list) || len(list)<=1 ? list : is_def(idx) - ? assert( _valid_idx(idx,0,len(list)) , "Invalid indices.") + ? assert( _valid_idx(idx,0,len(list[0])) , "Invalid indices or out of range.") let( sarr = _sort_general(list,idx) ) [for(i=[0:len(sarr)-1]) list[sarr[i]] ] : let(size = array_dim(list)) @@ -773,6 +845,31 @@ function sort(list, idx=undef) = ) : _sort_general(list); +function sort(list, idx=undef) = + !is_list(list) || len(list)<=1 ? list : + is_vector(list) + ? assert( _valid_idx(idx,0,len(list[0])) , str("Invalid indices or out of range. ",list)) + is_def(idx) + ? sort_vector_indexed([for(i=[0:len(list)-1]) [i, [list(i)] ] ]) + : sort_scalar(list) + : is_matrix(list) + ? list==[] || list[0]==[] ? list : + assert( _valid_idx(idx,0,len(list[0])) , "Invalid indices or out of range.") + is_def(idx) + ? sort_vector_indexed([for(i=[0:len(list)-1], li=[list[i]]) [i, [for(ind=idx) li(ind)] ] ]) + : sort_vector(list) + : list==[] || list[0]==[] ? list : + let( llen = [for(li=list) !is_list(li) || is_string(li) ? 0: len(li)], + m = min(llen), + M = max(llen) + ) + M==0 ? _simple_sort(list) : + assert( m>0 && _valid_idx(idx,m-1,M) , "Invalid indices or out of range.") + is_def(idx) + ? _sort_general(list,idx) + : let( ils = _sort_general(list, [m:M]) ) + [for(i=[0:len(list)-1]) list[ils[i]] ]; + // Function: sortidx() // Description: diff --git a/math.scad b/math.scad index 888d048..e5c4625 100644 --- a/math.scad +++ b/math.scad @@ -1175,7 +1175,7 @@ function deriv3(data, h=1, closed=false) = // Description: // Multiplies two complex numbers represented by 2D vectors. function C_times(z1,z2) = - assert( is_vector(z1+z2,2), "Complex numbers should be represented by 2D vectors." ) + assert( is_matrix([z1,z2],2,2), "Complex numbers should be represented by 2D vectors" ) [ z1.x*z2.x - z1.y*z2.y, z1.x*z2.y + z1.y*z2.x ]; // Function: C_div() diff --git a/quaternions.scad b/quaternions.scad index e21801c..44b0e29 100644 --- a/quaternions.scad +++ b/quaternions.scad @@ -27,7 +27,7 @@ function _Qreal(q) = q[3]; function _Qset(v,r) = concat( v, r ); // normalizes without checking -function _Qnorm(q) = q/norm(q); +function _Qunit(q) = q/norm(q); // Function: Q_is_quat() @@ -36,7 +36,7 @@ function _Qnorm(q) = q/norm(q); // 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 Q_is_quat(q) = is_vector(q,4) ;//&& ! approx(norm(q),0) ; // Function: Quat() @@ -101,7 +101,7 @@ function QuatXYZ(a=[0,0,0]) = qy = QuatY(a[1]), qz = QuatZ(a[2]) ) - Q_Mul(qz, Q_Mul(qy, qx)); + _Qmul(qz, _Qmul(qy, qx)); // Function: Q_From_to() @@ -202,32 +202,36 @@ function Q_Sub(a, 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)") + _Qmul(a,b); + +function _Qmul(a,b) = [ a[3]*b.x + a.x*b[3] + a.y*b.z - a.z*b.y, a[3]*b.y - a.x*b.z + a.y*b[3] + a.z*b.x, a[3]*b.z + a.x*b.y - a.y*b.x + a.z*b[3], - a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z, + a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z ]; - +// [ [a[3], -a.z, a.y, a.x], +// [ a.z, a[3], -a.x, a.y], +// [-a.y, a.x, a[3], a.z], +// [-a.x, -a.y, -a.z, a[3]] ]*[b.x,b.y,b.z,b[3]] + // Function: Q_Cumulative() // Usage: -// Q_Cumulative(v); +// Q_Cumulative(ql); // 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], select(_acc,-1))] - ) - ); +function Q_Cumulative(ql) = + assert( is_matrix(ql,undef,4) && len(ql)>0, "Invalid list of quaternions." ) + [for( i = 0, q = ql[0]; + i<=len(ql); + i = i+1, q = (i==len(ql))? 0: _Qmul(q,ql[i]) ) + q ]; // Function: Q_Dot() @@ -252,7 +256,7 @@ function Q_Neg(q) = // Q_Conj(q) // Description: Returns the conjugate of quaternion `q`. function Q_Conj(q) = - assert( Q_is_quat(q), "Invalid quaternion" ) + assert( Q_is_quat(q), str("Invalid quaternion",q) ) [-q.x, -q.y, -q.z, q[3]]; @@ -262,7 +266,7 @@ function Q_Conj(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) ) +// let(q = q/norm(q) ) [-q.x, -q.y, -q.z, q[3]]; @@ -283,7 +287,9 @@ function Q_Norm(q) = // Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1. function Q_Normalize(q) = assert( Q_is_quat(q) , "Invalid quaternion" ) - q/norm(q); + approx(_Qvec(q), [0,0,0]) + ? Q_Ident() + : q/norm(q); // Function: Q_Dist() @@ -318,31 +324,32 @@ function Q_Dist(q1, q2) = // Qrot(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(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) - let( - _dot = q1*q2, - q1 = q1/norm(q1), - q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), - dot = abs(_dot) - ) - ! is_finite(u) ? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] : - Q_Slerp(q1, q2, u, dot) - : _dot>0.9995 - ? _Qnorm(q1 + u*(q2-q1)) - : let( theta = u*acos(_dot), - q3 = _Qnorm(q2 - _dot*q1) - ) - _Qnorm(q1*cos(theta) + q3*sin(theta)); + assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(s)") + assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) + let( + _dot = q1*q2, + q1 = q1/norm(q1), + q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), + dot = abs(_dot), + q3 = dot>0.9995? q2: _Qunit(q2 - dot*q1) + ) + ! is_num(u) + ? [for (uu=u) _Qslerp(q1, q3, uu, dot)] + : _Qslerp(q1, q3, u, dot); +function _Qslerp(q1, q2, u, dot) = + dot>0.9995 + ? _Qunit(q1 + u*(q2-q1)) + : let( theta = u*acos(dot) ) + _Qunit(q1*cos(theta) + q2*sin(theta)); + -// Function: Q_Matrix3() +// Function: Q_to_matrix3() // Usage: -// Q_Matrix3(q); +// Q_to_matrix3(q); // Description: // Returns the 3x3 rotation matrix for the given normalized quaternion q. -function Q_Matrix3(q) = +function Q_to_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]], @@ -351,12 +358,12 @@ function Q_Matrix3(q) = ]; -// Function: Q_Matrix4() +// Function: Q_to_matrix4() // Usage: -// Q_Matrix4(q); +// Q_to_matrix4(q); // Description: // Returns the 4x4 rotation matrix for the given normalized quaternion q. -function Q_Matrix4(q) = +function Q_to_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], @@ -366,6 +373,35 @@ function Q_Matrix4(q) = ]; +// Function: Q_from_matrix() +// Usage: +// Q_from_matrix(M) +// Description: +// Returns a normalized quaternion corresponding to the rotation matrix M. +// M may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. +// The last row and last column of M are ignored for 4x4 matrices. +// It doesn't check whether M is in fact a rotation matrix. +// If M is not a rotation, the returned quaternion is unpredictable. +// +// based on https://en.wikipedia.org/wiki/Rotation_matrix +// +function Q_from_matrix(M) = + assert( is_matrix(M) && (len(M)==3 || len(M)==4) && (len(M[0])==3 || len(M[0])==4), + "The matrix should be 3x3 or 4x4") + let( tr = M[0][0]+M[1][1]+M[2][2] ) // M trace + tr>0 + ? let( r = sqrt(1+tr), s = -1/r/2 ) + _Qunit( _Qset([ M[1][2]-M[2][1], M[2][0]-M[0][2], M[0][1]-M[1][0] ]*s, r/2 ) ) + : let( + i = max_index([ M[0][0], M[1][1], M[2][2] ]), + r = sqrt(1 + 2*M[i][i] -M[0][0] -M[1][1] -M[2][2] ), + s = 1/r/2 + ) + i==0 ? _Qunit( _Qset( [ r/2, s*(M[1][0]+M[0][1]), s*(M[0][2]+M[2][0]) ], s*(M[2][1]-M[1][2])) ): + i==1 ? _Qunit( _Qset( [ s*(M[1][0]+M[0][1]), r/2, s*(M[2][1]+M[1][2]) ], s*(M[0][2]-M[2][0])) ) + : _Qunit( _Qset( [ s*(M[2][0]+M[0][2]), s*(M[1][2]+M[2][1]), r/2 ], s*(M[1][0]-M[0][1])) ) ; + + // Function: Q_Axis() // Usage: // Q_Axis(q) @@ -418,15 +454,15 @@ function Q_Angle(q1,q2) = // 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)) { + multmatrix(Q_to_matrix4(q)) { children(); } } function Qrot(q,p) = - is_undef(p)? Q_Matrix4(q) : + is_undef(p)? Q_to_matrix4(q) : is_vector(p)? Qrot(q,[p])[0] : - apply(Q_Matrix4(q), p); + apply(Q_to_matrix4(q), p); // Module: Qrot_copies() @@ -446,29 +482,6 @@ function Qrot(q,p) = module Qrot_copies(quats) for (q=quats) Qrot(q) children(); -// Function: Q_Rotation() -// Usage: -// Q_Rotation(R) -// Description: -// Returns a normalized quaternion corresponding to the rotation matrix R. -// R may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. -// The last row and last column of R are ignored for 4x4 matrices. -// It doesn't check whether R is in fact a rotation matrix. -// If R is not a rotation, the returned quaternion is an unpredictable quaternion . -function Q_Rotation(R) = - assert( is_matrix(R,3,3) || is_matrix(R,4,4) , - "Matrix is neither 3x3 nor 4x4") - let( tr = R[0][0]+R[1][1]+R[2][2] ) // R trace - tr>0 - ? let( r = 1+tr ) - _Qnorm( _Qset([ R[1][2]-R[2][1], R[2][0]-R[0][2], R[0][1]-R[1][0] ], -r ) ) - : let( i = max_index([ R[0][0], R[1][1], R[2][2] ]), - r = 1 + 2*R[i][i] -R[0][0] -R[1][1] -R[2][2] ) - i==0 ? _Qnorm( _Qset( [ 4*r, (R[1][0]+R[0][1]), (R[0][2]+R[2][0]) ], (R[2][1]-R[1][2])) ): - i==1 ? _Qnorm( _Qset( [ (R[1][0]+R[0][1]), 4*r, (R[2][1]+R[1][2]) ], (R[0][2]-R[2][0])) ): - _Qnorm( _Qset( [ (R[2][0]+R[0][2]), (R[1][2]+R[2][1]), 4*r ], (R[1][0]-R[0][1])) ) ; - - // Function&Module: Q_Rotation_path(q1, n, [q2]) // Usage: As a function // path = Q_Rotation_path(q1, n, q2); @@ -518,11 +531,11 @@ function Q_Rotation_path(q1, n=1, q2) = 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] + ? [for( i=0, dR=Q_to_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]; + dq = Q_pow( _Qmul( q2, Q_Inverse(q1) ), 1/n ), + dR = Q_to_matrix4(dq) ) + [for( i=0, R=Q_to_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)) @@ -565,8 +578,8 @@ function Q_Nlerp(q1,q2,u) = 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 ) ]; + ? _Qunit((1-u)*q1 + u*q2 ) + : [for (ui=u) _Qunit((1-ui)*q1 + ui*q2 ) ]; // Function: Q_Squad() @@ -616,6 +629,17 @@ function Q_Squad(q1,q2,q3,q4,u) = : [for(ui=u) Q_Slerp( Q_Slerp(q1,q4,ui), Q_Slerp(q2,q3,ui), 2*ui*(1-ui) ) ]; +function Q_Scubic(q1,q2,q3,q4,u) = + assert(is_finite(u) || is_range(u) || is_vector(u) , + "Invalid interpolation coefficient(s)" ) + is_num(u) + ? let( q12 = Q_Slerp(q1,q2,u), + q23 = Q_Slerp(q2,q3,u), + q34 = Q_Slerp(q3,q4,u) ) + Q_Slerp(Q_Slerp(q12,q23,u),Q_Slerp(q23,q34,u),u) + : [for(ui=u) Q_Scubic( q1,q2,q3,q4,ui) ]; + + // Function: Q_exp() // Usage: // q2 = Q_exp(q); diff --git a/tests/test_all.scad b/tests/test_all.scad new file mode 100644 index 0000000..fe6512b --- /dev/null +++ b/tests/test_all.scad @@ -0,0 +1,28 @@ +//include +include +include +include +include +include +include +include +include +include +include +include +include +include +//include +include +include +include +include +include +include +include +include +include +include +include + + diff --git a/tests/test_quaternions.scad b/tests/test_quaternions.scad index b1e5130..af19408 100644 --- a/tests/test_quaternions.scad +++ b/tests/test_quaternions.scad @@ -1,6 +1,80 @@ include <../std.scad> include <../strings.scad> +function plane_fit(points) = + assert( is_matrix(points,undef,3) , "Improper point list." ) + len(points)< 2 ? [] : + len(points)==3 ? plane3pt(points[0],points[1],points[2]) : + let( + A = [for(pi=points) concat(pi,-1) ], + qr = qr_factor(A), + R = select(qr[1],0,3), +x=[for(ri=R) echo(R=ri)0], +//y=[for(qi=qr[0]) echo(Q=qi)0], + s = back_substitute + ( R, + sum([for(qi=qr[0])[for(j=[0:3]) qi[j] ] ]) + ) +//, s0 = echo(ls=linear_solve(A,[for(p=points) 0])) +,w=s==[]? echo("s == []")0:echo(s=s)0 + ) + s==[] + ? // points are collinear + [] + : // plane through the origin? + approx(norm([s.x, s.y, s.z]), 0) + ? let( + k = max_index([for(i=[0:2]) abs(s[i]) ]), + A = [[1,0,0], for(pi=points) pi ], + b = [1, for(i=[1:len(points)]) 0], + s = linear_solve(A,b) +, y=echo(s2=s) + ) + s==[]? []: + concat(s, 0)/norm(s) + : s/norm([s.x, s.y, s.z]) ; + +function plane_fit(points) = + assert( is_matrix(points,undef,3) , "Improper point list." ) + len(points)< 2 ? [] : + len(points)==3 ? plane3pt(points[0],points[1],points[2]) : + let( + A = [for(pi=points) concat(pi,-1) ], + B = [ for(pi=points) [0,1] ], + sB = transpose(linear_solve(A,B)), + s = sB==[] ? [] + : norm(sB[1])N/2? 10*[1,1,1]: rands(-1,1,3) ]; +pf = plane_fit(pts); +echo(pf); +//pm = concat(sum(pts)/len(pts), -1); +//echo(pmfit=pm*pf); function rec_cmp(a,b,eps=1e-9) = typeof(a)!=typeof(b)? false : @@ -8,7 +82,18 @@ function rec_cmp(a,b,eps=1e-9) = is_list(a)? len(a)==len(b) && all([for (i=idx(a)) rec_cmp(a[i],b[i],eps=eps)]) : a == b; -function Qstandard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q; +function standardize(v) = + v==[] + ? [] + : sign(first_nonzero(v))*v; + +function first_nonzero(v) = + v==[] ? 0 + : is_num(v) ? v + : [for(vi=v) if(!is_list(vi) && ! approx(vi,0)) vi + else if(is_list(vi)) first_nonzero(vi), 0 ][0]; + +module assert_std(vc,ve) { assert_approx(standardize(vc),standardize(ve)); } module verify_f(actual,expected) { if (!rec_cmp(actual,expected)) { @@ -25,7 +110,7 @@ module verify_f(actual,expected) { module test_is_quat() { verify_f(Q_is_quat([0]),false); - verify_f(Q_is_quat([0,0,0,0]),false); + verify_f(Q_is_quat([0,0,0,0]),true); verify_f(Q_is_quat([1,0,2,0]),true); verify_f(Q_is_quat([1,0,2,0,0]),false); } @@ -104,7 +189,7 @@ test_QuatXYZ(); module test_Q_From_to() { verify_f(Q_Mul(Q_From_to([1,2,3], [4,5,2]),Q_From_to([4,5,2], [1,2,3])), Q_Ident()); - verify_f(Q_Matrix4(Q_From_to([1,2,3], [4,5,2])), rot(from=[1,2,3],to=[4,5,2])); + verify_f(Q_to_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])); } @@ -200,7 +285,7 @@ test_Q_Mul(); 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]]); } -test_Q_Cumulative(); +*test_Q_Cumulative(); module test_Q_Dot() { @@ -219,18 +304,18 @@ test_Q_Neg(); module test_Q_Conj() { + ang = rands(0,360,3); 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]); + verify_f(Q_Mul(Q_Conj(QuatXYZ(ang)),QuatXYZ(ang)),Q_Ident()); } test_Q_Conj(); module test_Q_Inverse() { - - verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]/sqrt(2)); - verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]/sqrt(2)); - verify_f(Q_Inverse(QuatXYZ([23,45,67])),Q_Conj(QuatXYZ([23,45,67]))); + verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]); + verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]); verify_f(Q_Mul(Q_Inverse(QuatXYZ([23,45,67])),QuatXYZ([23,45,67])),Q_Ident()); } test_Q_Inverse(); @@ -247,7 +332,7 @@ test_Q_Norm(); 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]); +// verify_f(Q_Normalize(QuatXYZ([23,45,67])),[-0.0533818345, 0.4143703268, 0.4360652669, 0.7970537592]); } test_Q_Normalize(); @@ -260,28 +345,47 @@ test_Q_Dist(); 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)); + u = rands(0,1,1)[0]; + ul = rands(0,1,5); + ul2 = [1,1,1,1,1]-ul; + a1 = rands(0,360,1)[0]; + a2 = rands(0,360,1)[0]; + a3 = rands(0,360,1)[0]; + verify_f(standardize(Q_Slerp(QuatX(a1),QuatY(a2),0.0)), standardize(QuatX(a1))); + verify_f(standardize(Q_Slerp(QuatX(45),QuatY(30),0.5)), + [0.1967063121, 0.1330377423, 0, 0.9713946602]); + verify_f(standardize(Q_Slerp(QuatX(a1),QuatY(a2),1.0)), standardize(QuatY(a2))); + verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),-QuatY(a2),u)), + standardize(Q_Slerp(QuatXYZ([a1,a2,0]), QuatY(a2),u))); + verify_f(standardize(Q_Slerp(QuatX(a1),QuatX(a1),u)), standardize(QuatX(a1))); + verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),QuatXYZ([a2,0,a1]),u)), + standardize(Q_Slerp(QuatXYZ([a2,0,a1]),QuatXYZ([a1,a2,0]),1-u))); + verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),QuatXYZ([a2,0,a1]),ul)), + standardize(Q_Slerp(QuatXYZ([a2,0,a1]),QuatXYZ([a1,a2,0]),ul2))); } test_Q_Slerp(); -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_to_matrix3() { + rotZ_37 = rot(37); + rotZ_37_3 = [for(i=[0:2]) [for(j=[0:2]) rotZ_37[i][j] ] ]; + angs = [12,-49,40]; + rot4 = rot(angs); + rot3 = [for(i=[0:2]) [for(j=[0:2]) rot4[i][j] ] ]; + verify_f(Q_to_matrix3(QuatZ(37)),rotZ_37_3); + verify_f(Q_to_matrix3(QuatXYZ(angs)),rot3); } -test_Q_Matrix3(); +test_Q_to_matrix3(); -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_to_matrix4() { + verify_f(Q_to_matrix4(QuatZ(37)),rot(37)); + verify_f(Q_to_matrix4(QuatZ(-49)),rot(-49)); + verify_f(Q_to_matrix4(QuatX(37)),rot([37,0,0])); + verify_f(Q_to_matrix4(QuatY(37)),rot([0,37,0])); + verify_f(Q_to_matrix4(QuatXYZ([12,34,56])),rot([12,34,56])); } -test_Q_Matrix4(); +test_Q_to_matrix4(); module test_Q_Axis() { @@ -321,23 +425,23 @@ module test_Qrot() { test_Qrot(); -module test_Q_Rotation() { - verify_f(Qstandard(Q_Rotation(Q_Matrix3(Quat([12,34,56],33)))),Qstandard(Quat([12,34,56],33))); - verify_f(Q_Matrix3(Q_Rotation(Q_Matrix3(QuatXYZ([12,34,56])))), - Q_Matrix3(QuatXYZ([12,34,56]))); +module test_Q_from_matrix() { + verify_f(standardize(Q_from_matrix(Q_to_matrix3(Quat([12,34,56],33)))),standardize(Quat([12,34,56],33))); + verify_f(Q_to_matrix3(Q_from_matrix(Q_to_matrix3(QuatXYZ([12,34,56])))), + Q_to_matrix3(QuatXYZ([12,34,56]))); } -test_Q_Rotation(); +test_Q_from_matrix(); 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), 5, QuatY(13.5))[0] , Q_to_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)[3] , xrot(11+(55-11)*3/4)); verify_f(Q_Rotation_path(QuatX(11), 5)[4] , xrot(55)); } @@ -347,48 +451,54 @@ 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_Rotation_path(QuatX(135), 16, QuatY(13.5))[8] , + Q_to_matrix4(Q_Nlerp(QuatX(135), QuatY(13.5),0.5))); verify_f(Q_Nlerp(QuatX(45),QuatY(30),1.0),QuatY(30)); } test_Q_Nlerp(); module test_Q_Squad() { - verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(90),QuatY(30),0.0),QuatX(45)); + u = rands(0,1,5); + su = [1,1,1,1,1]-u; + verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(32),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), + 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)); + verify_f( Q_Squad(QuatY(10),QuatZ(20),QuatX(32),QuatXYZ([210,120,30]),u[0] ), + Q_Squad(QuatXYZ([210,120,30]),QuatX(32),QuatZ(20),QuatY(10),1-u[0] ) ); + verify_f( Q_Squad(QuatY(10),QuatZ(20),QuatX(32),QuatXYZ([210,120,30]),u ), + Q_Squad(QuatXYZ([210,120,30]),QuatX(32),QuatZ(20),QuatY(10),su ) ); } test_Q_Squad(); module test_Q_exp() { + q=QuatXYZ(rands(0,360,3)); 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)))); + verify_f(Q_exp(Q_ln([1,2,3,4])), [1,2,3,4]); + verify_f(Q_exp(Q_ln(q)), q); + verify_f(Q_exp(q+Q_Inverse(q)),Q_Mul(Q_exp(q),Q_exp(Q_Inverse(q)))); } test_Q_exp(); module test_Q_ln() { + q = QuatXYZ(rands(0,360,3)); 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]); + verify_f(Q_ln(Q_exp(q)), q); + verify_f(Q_ln(q)+Q_ln(Q_Conj(q)), [0,0,0,0]); } test_Q_ln(); module test_Q_pow() { - q = Quat([1,2,3],77); + q = QuatXYZ(rands(0,360,3)); verify_f(Q_pow(q,1), q); verify_f(Q_pow(q,0), Q_Ident()); verify_f(Q_pow(q,-1), Q_Inverse(q)); From 4a366967f96c29c61116125b0803a1dd405e1f91 Mon Sep 17 00:00:00 2001 From: RonaldoCMP Date: Wed, 26 Aug 2020 14:07:53 +0100 Subject: [PATCH 2/5] Revert "Correction of C_times validation" This reverts commit 1ced82f16c80751bc1d49bdb6bc5ff425488e41d. --- arrays.scad | 125 +++-------------------- math.scad | 2 +- quaternions.scad | 170 ++++++++++++++----------------- tests/test_all.scad | 28 ------ tests/test_quaternions.scad | 196 ++++++++---------------------------- 5 files changed, 131 insertions(+), 390 deletions(-) delete mode 100644 tests/test_all.scad diff --git a/arrays.scad b/arrays.scad index 3ae7a88..835fd65 100644 --- a/arrays.scad +++ b/arrays.scad @@ -708,108 +708,36 @@ function _sort_vectors4(arr) = y ] ) concat( _sort_vectors4(lesser), equal, _sort_vectors4(greater) ); -// sort a list of vectors -function _sort_vectors(arr, _i=0) = - len(arr)<=1 || _i>=len(arr[0]) ? arr : - let( - pivot = arr[floor(len(arr)/2)][_i], - lesser = [ for (entry=arr) if (entry[_i] < pivot ) entry ], - equal = [ for (entry=arr) if (entry[_i] == pivot ) entry ], - greater = [ for (entry=arr) if (entry[_i] > pivot ) entry ] - ) - concat( - _sort_vectors(lesser, _i ), - _sort_vectors(equal, _i+1 ), - _sort_vectors(greater, _i ) ); - -// given pairs of an index and a vector, return the list of indices of the list sorted by the vectors -function _sort_vectors_indexed(arr, _i=0) = - arr==[] ? [] : - len(arr)==1 || _i>=len(arr[0][1]) ? [for(ai=arr) ai[0]] : - let( - pivot = arr[floor(len(arr)/2)][1][_i], - lesser = [ for (entry=arr) if (entry[1][_i] < pivot ) entry ], - equal = [ for (entry=arr) if (entry[1][_i] == pivot ) entry ], - greater = [ for (entry=arr) if (entry[1][_i] > pivot ) entry ] - ) - concat( - _sort_vectors_indexed(lesser, _i ), - _sort_vectors_indexed(equal, _i+1 ), - _sort_vectors_indexed(greater, _i ) ); - // when idx==undef, returns the sorted array // otherwise, returns the indices of the sorted array function _sort_general(arr, idx=undef) = - len(arr)<=1 ? arr : + (len(arr)<=1) ? arr : is_undef(idx) - ? _simple_sort(arr) -// : _lexical_sort(arr) + ? _sort_scalar(arr) : let( arrind=[for(k=[0:len(arr)-1], ark=[arr[k]]) [ k, [for (i=idx) ark[i]] ] ] ) - _indexed_sort(arrind ); - -// sort simple lists with compare_vals() -function _simple_sort(arr) = - arr==[] || len(arr)==1 ? arr : - let( - pivot = arr[floor(len(arr)/2)], - lesser = [ for (entry=arr) if (compare_vals(entry, pivot) <0 ) entry ], - equal = [ for (entry=arr) if (compare_vals(entry, pivot)==0 ) entry ], - greater = [ for (entry=arr) if (compare_vals(entry, pivot) >0 ) entry ] - ) - concat( - _simple_sort(lesser), - equal, - _simple_sort(greater) - ); - - + _indexed_sort(arrind); + // given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair -// it uses compare_vals() -function _lexical_sort(arr, _i=0) = - arr==[] || len(arr)==1 || _i>=len(arr[0]) ? arr : +// the sorting is done using compare_vals() +function _indexed_sort(arrind) = + arrind==[] ? [] : len(arrind)==1? [arrind[0][0]] : + let( pivot = arrind[floor(len(arrind)/2)][1] ) let( - pivot = arr[floor(len(arr)/2)][_i], - lesser = [ for (entry=arr) if (compare_vals(entry[_i], pivot) <0 ) entry ], - equal = [ for (entry=arr) if (compare_vals(entry[_i], pivot)==0 ) entry ], - greater = [ for (entry=arr) if (compare_vals(entry[_i], pivot) >0 ) entry ] + lesser = [ for (entry=arrind) if (compare_vals(entry[1], pivot) <0 ) entry ], + equal = [ for (entry=arrind) if (compare_vals(entry[1], pivot)==0 ) entry[0] ], + greater = [ for (entry=arrind) if (compare_vals(entry[1], pivot) >0 ) entry ] ) - concat( - _lexical_sort(lesser, _i ), - _lexical_sort(equal, _i+1 ), - _lexical_sort(greater, _i ) - ); - - -// given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair -// it uses compare_vals() -function _indexed_sort(arr, _i=0) = - arr==[] ? [] : - len(arr)==1? [arr[0][0]] : - _i>=len(arr[0][1]) ? [for(ai=arr) ai[0]] : - let( - pivot = arr[floor(len(arr)/2)][1][_i], - lesser = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot) <0 ) entry ], - equal = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot)==0 ) entry ], - greater = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot) >0 ) entry ] - ) - concat( - _indexed_sort(lesser, _i ), - _indexed_sort(equal, _i+1 ), - _indexed_sort(greater, _i ) ); + concat(_indexed_sort(lesser), equal, _indexed_sort(greater)); // returns true for valid index specifications idx in the interval [imin, imax) // note that idx can't have any value greater or EQUAL to imax -// this allows imax=INF as a bound to numerical lists function _valid_idx(idx,imin,imax) = is_undef(idx) || ( is_finite(idx) && idx>=imin && idx< imax ) || ( is_list(idx) && min(idx)>=imin && max(idx)< imax ) - || ( ! is_list(idx) // it implicitly is a range - && (idx[1]>0 && idx[0]>=imin && idx[2]< imax) - || - (idx[0]=imin) ); + || ( valid_range(idx) && idx[0]>=imin && idx[2]< imax ); // Function: sort() @@ -830,7 +758,7 @@ function _valid_idx(idx,imin,imax) = function sort(list, idx=undef) = !is_list(list) || len(list)<=1 ? list : is_def(idx) - ? assert( _valid_idx(idx,0,len(list[0])) , "Invalid indices or out of range.") + ? assert( _valid_idx(idx,0,len(list)) , "Invalid indices.") let( sarr = _sort_general(list,idx) ) [for(i=[0:len(sarr)-1]) list[sarr[i]] ] : let(size = array_dim(list)) @@ -845,31 +773,6 @@ function sort(list, idx=undef) = ) : _sort_general(list); -function sort(list, idx=undef) = - !is_list(list) || len(list)<=1 ? list : - is_vector(list) - ? assert( _valid_idx(idx,0,len(list[0])) , str("Invalid indices or out of range. ",list)) - is_def(idx) - ? sort_vector_indexed([for(i=[0:len(list)-1]) [i, [list(i)] ] ]) - : sort_scalar(list) - : is_matrix(list) - ? list==[] || list[0]==[] ? list : - assert( _valid_idx(idx,0,len(list[0])) , "Invalid indices or out of range.") - is_def(idx) - ? sort_vector_indexed([for(i=[0:len(list)-1], li=[list[i]]) [i, [for(ind=idx) li(ind)] ] ]) - : sort_vector(list) - : list==[] || list[0]==[] ? list : - let( llen = [for(li=list) !is_list(li) || is_string(li) ? 0: len(li)], - m = min(llen), - M = max(llen) - ) - M==0 ? _simple_sort(list) : - assert( m>0 && _valid_idx(idx,m-1,M) , "Invalid indices or out of range.") - is_def(idx) - ? _sort_general(list,idx) - : let( ils = _sort_general(list, [m:M]) ) - [for(i=[0:len(list)-1]) list[ils[i]] ]; - // Function: sortidx() // Description: diff --git a/math.scad b/math.scad index e5c4625..888d048 100644 --- a/math.scad +++ b/math.scad @@ -1175,7 +1175,7 @@ function deriv3(data, h=1, closed=false) = // Description: // Multiplies two complex numbers represented by 2D vectors. function C_times(z1,z2) = - assert( is_matrix([z1,z2],2,2), "Complex numbers should be represented by 2D vectors" ) + assert( is_vector(z1+z2,2), "Complex numbers should be represented by 2D vectors." ) [ z1.x*z2.x - z1.y*z2.y, z1.x*z2.y + z1.y*z2.x ]; // Function: C_div() diff --git a/quaternions.scad b/quaternions.scad index 44b0e29..e21801c 100644 --- a/quaternions.scad +++ b/quaternions.scad @@ -27,7 +27,7 @@ function _Qreal(q) = q[3]; function _Qset(v,r) = concat( v, r ); // normalizes without checking -function _Qunit(q) = q/norm(q); +function _Qnorm(q) = q/norm(q); // Function: Q_is_quat() @@ -36,7 +36,7 @@ function _Qunit(q) = q/norm(q); // 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 Q_is_quat(q) = is_vector(q,4) && ! approx(norm(q),0) ; // Function: Quat() @@ -101,7 +101,7 @@ function QuatXYZ(a=[0,0,0]) = qy = QuatY(a[1]), qz = QuatZ(a[2]) ) - _Qmul(qz, _Qmul(qy, qx)); + Q_Mul(qz, Q_Mul(qy, qx)); // Function: Q_From_to() @@ -202,36 +202,32 @@ function Q_Sub(a, 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)") - _Qmul(a,b); - -function _Qmul(a,b) = [ a[3]*b.x + a.x*b[3] + a.y*b.z - a.z*b.y, a[3]*b.y - a.x*b.z + a.y*b[3] + a.z*b.x, a[3]*b.z + a.x*b.y - a.y*b.x + a.z*b[3], - a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z + a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z, ]; -// [ [a[3], -a.z, a.y, a.x], -// [ a.z, a[3], -a.x, a.y], -// [-a.y, a.x, a[3], a.z], -// [-a.x, -a.y, -a.z, a[3]] ]*[b.x,b.y,b.z,b[3]] - + // Function: Q_Cumulative() // Usage: -// Q_Cumulative(ql); +// 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(ql) = - assert( is_matrix(ql,undef,4) && len(ql)>0, "Invalid list of quaternions." ) - [for( i = 0, q = ql[0]; - i<=len(ql); - i = i+1, q = (i==len(ql))? 0: _Qmul(q,ql[i]) ) - q ]; +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], select(_acc,-1))] + ) + ); // Function: Q_Dot() @@ -256,7 +252,7 @@ function Q_Neg(q) = // Q_Conj(q) // Description: Returns the conjugate of quaternion `q`. function Q_Conj(q) = - assert( Q_is_quat(q), str("Invalid quaternion",q) ) + assert( Q_is_quat(q), "Invalid quaternion" ) [-q.x, -q.y, -q.z, q[3]]; @@ -266,7 +262,7 @@ function Q_Conj(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 = q/norm(q) ) + let(q = _Qnorm(q) ) [-q.x, -q.y, -q.z, q[3]]; @@ -287,9 +283,7 @@ function Q_Norm(q) = // Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1. function Q_Normalize(q) = assert( Q_is_quat(q) , "Invalid quaternion" ) - approx(_Qvec(q), [0,0,0]) - ? Q_Ident() - : q/norm(q); + q/norm(q); // Function: Q_Dist() @@ -324,32 +318,31 @@ function Q_Dist(q1, q2) = // Qrot(q) right(80) cube([10,10,1]); // #sphere(r=80); function Q_Slerp(q1, q2, u, _dot) = - assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(s)") - assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) - let( - _dot = q1*q2, - q1 = q1/norm(q1), - q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), - dot = abs(_dot), - q3 = dot>0.9995? q2: _Qunit(q2 - dot*q1) - ) - ! is_num(u) - ? [for (uu=u) _Qslerp(q1, q3, uu, dot)] - : _Qslerp(q1, q3, u, dot); + is_undef(_dot) + ? assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(s)") + assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) + let( + _dot = q1*q2, + q1 = q1/norm(q1), + q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), + dot = abs(_dot) + ) + ! is_finite(u) ? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] : + Q_Slerp(q1, q2, u, dot) + : _dot>0.9995 + ? _Qnorm(q1 + u*(q2-q1)) + : let( theta = u*acos(_dot), + q3 = _Qnorm(q2 - _dot*q1) + ) + _Qnorm(q1*cos(theta) + q3*sin(theta)); -function _Qslerp(q1, q2, u, dot) = - dot>0.9995 - ? _Qunit(q1 + u*(q2-q1)) - : let( theta = u*acos(dot) ) - _Qunit(q1*cos(theta) + q2*sin(theta)); - -// Function: Q_to_matrix3() +// Function: Q_Matrix3() // Usage: -// Q_to_matrix3(q); +// Q_Matrix3(q); // Description: // Returns the 3x3 rotation matrix for the given normalized quaternion q. -function Q_to_matrix3(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]], @@ -358,12 +351,12 @@ function Q_to_matrix3(q) = ]; -// Function: Q_to_matrix4() +// Function: Q_Matrix4() // Usage: -// Q_to_matrix4(q); +// Q_Matrix4(q); // Description: // Returns the 4x4 rotation matrix for the given normalized quaternion q. -function Q_to_matrix4(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], @@ -373,35 +366,6 @@ function Q_to_matrix4(q) = ]; -// Function: Q_from_matrix() -// Usage: -// Q_from_matrix(M) -// Description: -// Returns a normalized quaternion corresponding to the rotation matrix M. -// M may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. -// The last row and last column of M are ignored for 4x4 matrices. -// It doesn't check whether M is in fact a rotation matrix. -// If M is not a rotation, the returned quaternion is unpredictable. -// -// based on https://en.wikipedia.org/wiki/Rotation_matrix -// -function Q_from_matrix(M) = - assert( is_matrix(M) && (len(M)==3 || len(M)==4) && (len(M[0])==3 || len(M[0])==4), - "The matrix should be 3x3 or 4x4") - let( tr = M[0][0]+M[1][1]+M[2][2] ) // M trace - tr>0 - ? let( r = sqrt(1+tr), s = -1/r/2 ) - _Qunit( _Qset([ M[1][2]-M[2][1], M[2][0]-M[0][2], M[0][1]-M[1][0] ]*s, r/2 ) ) - : let( - i = max_index([ M[0][0], M[1][1], M[2][2] ]), - r = sqrt(1 + 2*M[i][i] -M[0][0] -M[1][1] -M[2][2] ), - s = 1/r/2 - ) - i==0 ? _Qunit( _Qset( [ r/2, s*(M[1][0]+M[0][1]), s*(M[0][2]+M[2][0]) ], s*(M[2][1]-M[1][2])) ): - i==1 ? _Qunit( _Qset( [ s*(M[1][0]+M[0][1]), r/2, s*(M[2][1]+M[1][2]) ], s*(M[0][2]-M[2][0])) ) - : _Qunit( _Qset( [ s*(M[2][0]+M[0][2]), s*(M[1][2]+M[2][1]), r/2 ], s*(M[1][0]-M[0][1])) ) ; - - // Function: Q_Axis() // Usage: // Q_Axis(q) @@ -454,15 +418,15 @@ function Q_Angle(q1,q2) = // q = QuatXYZ([45,35,10]); // pts = Qrot(q, p=[[2,3,4], [4,5,6], [9,2,3]]); module Qrot(q) { - multmatrix(Q_to_matrix4(q)) { + multmatrix(Q_Matrix4(q)) { children(); } } function Qrot(q,p) = - is_undef(p)? Q_to_matrix4(q) : + is_undef(p)? Q_Matrix4(q) : is_vector(p)? Qrot(q,[p])[0] : - apply(Q_to_matrix4(q), p); + apply(Q_Matrix4(q), p); // Module: Qrot_copies() @@ -482,6 +446,29 @@ function Qrot(q,p) = module Qrot_copies(quats) for (q=quats) Qrot(q) children(); +// Function: Q_Rotation() +// Usage: +// Q_Rotation(R) +// Description: +// Returns a normalized quaternion corresponding to the rotation matrix R. +// R may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. +// The last row and last column of R are ignored for 4x4 matrices. +// It doesn't check whether R is in fact a rotation matrix. +// If R is not a rotation, the returned quaternion is an unpredictable quaternion . +function Q_Rotation(R) = + assert( is_matrix(R,3,3) || is_matrix(R,4,4) , + "Matrix is neither 3x3 nor 4x4") + let( tr = R[0][0]+R[1][1]+R[2][2] ) // R trace + tr>0 + ? let( r = 1+tr ) + _Qnorm( _Qset([ R[1][2]-R[2][1], R[2][0]-R[0][2], R[0][1]-R[1][0] ], -r ) ) + : let( i = max_index([ R[0][0], R[1][1], R[2][2] ]), + r = 1 + 2*R[i][i] -R[0][0] -R[1][1] -R[2][2] ) + i==0 ? _Qnorm( _Qset( [ 4*r, (R[1][0]+R[0][1]), (R[0][2]+R[2][0]) ], (R[2][1]-R[1][2])) ): + i==1 ? _Qnorm( _Qset( [ (R[1][0]+R[0][1]), 4*r, (R[2][1]+R[1][2]) ], (R[0][2]-R[2][0])) ): + _Qnorm( _Qset( [ (R[2][0]+R[0][2]), (R[1][2]+R[2][1]), 4*r ], (R[1][0]-R[0][1])) ) ; + + // Function&Module: Q_Rotation_path(q1, n, [q2]) // Usage: As a function // path = Q_Rotation_path(q1, n, q2); @@ -531,11 +518,11 @@ function Q_Rotation_path(q1, n=1, q2) = 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_to_matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R] + ? [for( i=0, dR=Q_Matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R] : let( q2 = Q_Normalize( q1*q2<0 ? -q2: q2 ), - dq = Q_pow( _Qmul( q2, Q_Inverse(q1) ), 1/n ), - dR = Q_to_matrix4(dq) ) - [for( i=0, R=Q_to_matrix4(q1); i<=n; i=i+1, R=dR*R ) R]; + 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)) @@ -578,8 +565,8 @@ function Q_Nlerp(q1,q2,u) = let( q1 = Q_Normalize(q1), q2 = Q_Normalize(q2) ) is_num(u) - ? _Qunit((1-u)*q1 + u*q2 ) - : [for (ui=u) _Qunit((1-ui)*q1 + ui*q2 ) ]; + ? _Qnorm((1-u)*q1 + u*q2 ) + : [for (ui=u) _Qnorm((1-ui)*q1 + ui*q2 ) ]; // Function: Q_Squad() @@ -629,17 +616,6 @@ function Q_Squad(q1,q2,q3,q4,u) = : [for(ui=u) Q_Slerp( Q_Slerp(q1,q4,ui), Q_Slerp(q2,q3,ui), 2*ui*(1-ui) ) ]; -function Q_Scubic(q1,q2,q3,q4,u) = - assert(is_finite(u) || is_range(u) || is_vector(u) , - "Invalid interpolation coefficient(s)" ) - is_num(u) - ? let( q12 = Q_Slerp(q1,q2,u), - q23 = Q_Slerp(q2,q3,u), - q34 = Q_Slerp(q3,q4,u) ) - Q_Slerp(Q_Slerp(q12,q23,u),Q_Slerp(q23,q34,u),u) - : [for(ui=u) Q_Scubic( q1,q2,q3,q4,ui) ]; - - // Function: Q_exp() // Usage: // q2 = Q_exp(q); diff --git a/tests/test_all.scad b/tests/test_all.scad deleted file mode 100644 index fe6512b..0000000 --- a/tests/test_all.scad +++ /dev/null @@ -1,28 +0,0 @@ -//include -include -include -include -include -include -include -include -include -include -include -include -include -include -//include -include -include -include -include -include -include -include -include -include -include -include - - diff --git a/tests/test_quaternions.scad b/tests/test_quaternions.scad index af19408..b1e5130 100644 --- a/tests/test_quaternions.scad +++ b/tests/test_quaternions.scad @@ -1,80 +1,6 @@ include <../std.scad> include <../strings.scad> -function plane_fit(points) = - assert( is_matrix(points,undef,3) , "Improper point list." ) - len(points)< 2 ? [] : - len(points)==3 ? plane3pt(points[0],points[1],points[2]) : - let( - A = [for(pi=points) concat(pi,-1) ], - qr = qr_factor(A), - R = select(qr[1],0,3), -x=[for(ri=R) echo(R=ri)0], -//y=[for(qi=qr[0]) echo(Q=qi)0], - s = back_substitute - ( R, - sum([for(qi=qr[0])[for(j=[0:3]) qi[j] ] ]) - ) -//, s0 = echo(ls=linear_solve(A,[for(p=points) 0])) -,w=s==[]? echo("s == []")0:echo(s=s)0 - ) - s==[] - ? // points are collinear - [] - : // plane through the origin? - approx(norm([s.x, s.y, s.z]), 0) - ? let( - k = max_index([for(i=[0:2]) abs(s[i]) ]), - A = [[1,0,0], for(pi=points) pi ], - b = [1, for(i=[1:len(points)]) 0], - s = linear_solve(A,b) -, y=echo(s2=s) - ) - s==[]? []: - concat(s, 0)/norm(s) - : s/norm([s.x, s.y, s.z]) ; - -function plane_fit(points) = - assert( is_matrix(points,undef,3) , "Improper point list." ) - len(points)< 2 ? [] : - len(points)==3 ? plane3pt(points[0],points[1],points[2]) : - let( - A = [for(pi=points) concat(pi,-1) ], - B = [ for(pi=points) [0,1] ], - sB = transpose(linear_solve(A,B)), - s = sB==[] ? [] - : norm(sB[1])N/2? 10*[1,1,1]: rands(-1,1,3) ]; -pf = plane_fit(pts); -echo(pf); -//pm = concat(sum(pts)/len(pts), -1); -//echo(pmfit=pm*pf); function rec_cmp(a,b,eps=1e-9) = typeof(a)!=typeof(b)? false : @@ -82,18 +8,7 @@ function rec_cmp(a,b,eps=1e-9) = is_list(a)? len(a)==len(b) && all([for (i=idx(a)) rec_cmp(a[i],b[i],eps=eps)]) : a == b; -function standardize(v) = - v==[] - ? [] - : sign(first_nonzero(v))*v; - -function first_nonzero(v) = - v==[] ? 0 - : is_num(v) ? v - : [for(vi=v) if(!is_list(vi) && ! approx(vi,0)) vi - else if(is_list(vi)) first_nonzero(vi), 0 ][0]; - -module assert_std(vc,ve) { assert_approx(standardize(vc),standardize(ve)); } +function Qstandard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q; module verify_f(actual,expected) { if (!rec_cmp(actual,expected)) { @@ -110,7 +25,7 @@ module verify_f(actual,expected) { module test_is_quat() { verify_f(Q_is_quat([0]),false); - verify_f(Q_is_quat([0,0,0,0]),true); + 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); } @@ -189,7 +104,7 @@ test_QuatXYZ(); module test_Q_From_to() { verify_f(Q_Mul(Q_From_to([1,2,3], [4,5,2]),Q_From_to([4,5,2], [1,2,3])), Q_Ident()); - verify_f(Q_to_matrix4(Q_From_to([1,2,3], [4,5,2])), rot(from=[1,2,3],to=[4,5,2])); + 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])); } @@ -285,7 +200,7 @@ test_Q_Mul(); 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]]); } -*test_Q_Cumulative(); +test_Q_Cumulative(); module test_Q_Dot() { @@ -304,18 +219,18 @@ test_Q_Neg(); module test_Q_Conj() { - ang = rands(0,360,3); 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]); - verify_f(Q_Mul(Q_Conj(QuatXYZ(ang)),QuatXYZ(ang)),Q_Ident()); } test_Q_Conj(); module test_Q_Inverse() { - verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]); - verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]); + + verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]/sqrt(2)); + verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]/sqrt(2)); + verify_f(Q_Inverse(QuatXYZ([23,45,67])),Q_Conj(QuatXYZ([23,45,67]))); verify_f(Q_Mul(Q_Inverse(QuatXYZ([23,45,67])),QuatXYZ([23,45,67])),Q_Ident()); } test_Q_Inverse(); @@ -332,7 +247,7 @@ test_Q_Norm(); 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]); + verify_f(Q_Normalize(QuatXYZ([23,45,67])),[-0.0533818345, 0.4143703268, 0.4360652669, 0.7970537592]); } test_Q_Normalize(); @@ -345,47 +260,28 @@ test_Q_Dist(); module test_Q_Slerp() { - u = rands(0,1,1)[0]; - ul = rands(0,1,5); - ul2 = [1,1,1,1,1]-ul; - a1 = rands(0,360,1)[0]; - a2 = rands(0,360,1)[0]; - a3 = rands(0,360,1)[0]; - verify_f(standardize(Q_Slerp(QuatX(a1),QuatY(a2),0.0)), standardize(QuatX(a1))); - verify_f(standardize(Q_Slerp(QuatX(45),QuatY(30),0.5)), - [0.1967063121, 0.1330377423, 0, 0.9713946602]); - verify_f(standardize(Q_Slerp(QuatX(a1),QuatY(a2),1.0)), standardize(QuatY(a2))); - verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),-QuatY(a2),u)), - standardize(Q_Slerp(QuatXYZ([a1,a2,0]), QuatY(a2),u))); - verify_f(standardize(Q_Slerp(QuatX(a1),QuatX(a1),u)), standardize(QuatX(a1))); - verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),QuatXYZ([a2,0,a1]),u)), - standardize(Q_Slerp(QuatXYZ([a2,0,a1]),QuatXYZ([a1,a2,0]),1-u))); - verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),QuatXYZ([a2,0,a1]),ul)), - standardize(Q_Slerp(QuatXYZ([a2,0,a1]),QuatXYZ([a1,a2,0]),ul2))); + 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)); } test_Q_Slerp(); -module test_Q_to_matrix3() { - rotZ_37 = rot(37); - rotZ_37_3 = [for(i=[0:2]) [for(j=[0:2]) rotZ_37[i][j] ] ]; - angs = [12,-49,40]; - rot4 = rot(angs); - rot3 = [for(i=[0:2]) [for(j=[0:2]) rot4[i][j] ] ]; - verify_f(Q_to_matrix3(QuatZ(37)),rotZ_37_3); - verify_f(Q_to_matrix3(QuatXYZ(angs)),rot3); +module test_Q_Matrix3() { + verify_f(Q_Matrix3(QuatZ(37)),rot(37,planar=true)); + verify_f(Q_Matrix3(QuatZ(-49)),rot(-49,planar=true)); } -test_Q_to_matrix3(); +test_Q_Matrix3(); -module test_Q_to_matrix4() { - verify_f(Q_to_matrix4(QuatZ(37)),rot(37)); - verify_f(Q_to_matrix4(QuatZ(-49)),rot(-49)); - verify_f(Q_to_matrix4(QuatX(37)),rot([37,0,0])); - verify_f(Q_to_matrix4(QuatY(37)),rot([0,37,0])); - verify_f(Q_to_matrix4(QuatXYZ([12,34,56])),rot([12,34,56])); +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])); } -test_Q_to_matrix4(); +test_Q_Matrix4(); module test_Q_Axis() { @@ -425,23 +321,23 @@ module test_Qrot() { test_Qrot(); -module test_Q_from_matrix() { - verify_f(standardize(Q_from_matrix(Q_to_matrix3(Quat([12,34,56],33)))),standardize(Quat([12,34,56],33))); - verify_f(Q_to_matrix3(Q_from_matrix(Q_to_matrix3(QuatXYZ([12,34,56])))), - Q_to_matrix3(QuatXYZ([12,34,56]))); +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_from_matrix(); +test_Q_Rotation(); module test_Q_Rotation_path() { - verify_f(Q_Rotation_path(QuatX(135), 5, QuatY(13.5))[0] , Q_to_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), 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)[3] , xrot(11+(55-11)*3/4)); verify_f(Q_Rotation_path(QuatX(11), 5)[4] , xrot(55)); } @@ -451,54 +347,48 @@ 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_to_matrix4(Q_Nlerp(QuatX(135), QuatY(13.5),0.5))); + verify_f(Q_Rotation_path(QuatX(135), 16, QuatY(13.5))[8] , Q_Matrix4(Q_Nlerp(QuatX(135), QuatY(13.5),0.5))); verify_f(Q_Nlerp(QuatX(45),QuatY(30),1.0),QuatY(30)); } test_Q_Nlerp(); module test_Q_Squad() { - u = rands(0,1,5); - su = [1,1,1,1,1]-u; - verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(32),QuatY(30),0.0),QuatX(45)); + 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), + 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(10),QuatZ(20),QuatX(32),QuatXYZ([210,120,30]),u[0] ), - Q_Squad(QuatXYZ([210,120,30]),QuatX(32),QuatZ(20),QuatY(10),1-u[0] ) ); - verify_f( Q_Squad(QuatY(10),QuatZ(20),QuatX(32),QuatXYZ([210,120,30]),u ), - Q_Squad(QuatXYZ([210,120,30]),QuatX(32),QuatZ(20),QuatY(10),su ) ); + verify_f(Q_Squad(QuatY(0),QuatY(0),QuatX(120),QuatX(120),0.3), + Q_Slerp(QuatY(0),QuatX(120),0.3)); } test_Q_Squad(); module test_Q_exp() { - q=QuatXYZ(rands(0,360,3)); 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,4])), [1,2,3,4]); - verify_f(Q_exp(Q_ln(q)), q); - verify_f(Q_exp(q+Q_Inverse(q)),Q_Mul(Q_exp(q),Q_exp(Q_Inverse(q)))); + verify_f(Q_exp(Q_ln([1,2,3,0])), [1,2,3,0]); + verify_f(Q_exp(Q_ln(QuatXYZ([31,27,34]))), QuatXYZ([31,27,34])); + let(q=QuatXYZ([12,23,34])) + verify_f(Q_exp(q+Q_Inverse(q)),Q_Mul(Q_exp(q),Q_exp(Q_Inverse(q)))); } test_Q_exp(); module test_Q_ln() { - q = QuatXYZ(rands(0,360,3)); 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(q)), q); - verify_f(Q_ln(q)+Q_ln(Q_Conj(q)), [0,0,0,0]); + verify_f(Q_ln(Q_exp(QuatXYZ([13,37,43]))), QuatXYZ([13,37,43])); + verify_f(Q_ln(QuatXYZ([12,23,34]))+Q_ln(Q_Inverse(QuatXYZ([12,23,34]))), [0,0,0,0]); } test_Q_ln(); module test_Q_pow() { - q = QuatXYZ(rands(0,360,3)); + 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)); From e8499baba185848ff6f6f1745181a33573ee5c58 Mon Sep 17 00:00:00 2001 From: RonaldoCMP Date: Wed, 26 Aug 2020 14:09:12 +0100 Subject: [PATCH 3/5] Revert "Revert "Correction of C_times validation"" This reverts commit 4a366967f96c29c61116125b0803a1dd405e1f91. --- arrays.scad | 127 ++++++++++++++++++++--- math.scad | 2 +- quaternions.scad | 170 +++++++++++++++++-------------- tests/test_all.scad | 28 ++++++ tests/test_quaternions.scad | 196 ++++++++++++++++++++++++++++-------- 5 files changed, 391 insertions(+), 132 deletions(-) create mode 100644 tests/test_all.scad diff --git a/arrays.scad b/arrays.scad index 835fd65..3ae7a88 100644 --- a/arrays.scad +++ b/arrays.scad @@ -708,36 +708,108 @@ function _sort_vectors4(arr) = y ] ) concat( _sort_vectors4(lesser), equal, _sort_vectors4(greater) ); +// sort a list of vectors +function _sort_vectors(arr, _i=0) = + len(arr)<=1 || _i>=len(arr[0]) ? arr : + let( + pivot = arr[floor(len(arr)/2)][_i], + lesser = [ for (entry=arr) if (entry[_i] < pivot ) entry ], + equal = [ for (entry=arr) if (entry[_i] == pivot ) entry ], + greater = [ for (entry=arr) if (entry[_i] > pivot ) entry ] + ) + concat( + _sort_vectors(lesser, _i ), + _sort_vectors(equal, _i+1 ), + _sort_vectors(greater, _i ) ); + +// given pairs of an index and a vector, return the list of indices of the list sorted by the vectors +function _sort_vectors_indexed(arr, _i=0) = + arr==[] ? [] : + len(arr)==1 || _i>=len(arr[0][1]) ? [for(ai=arr) ai[0]] : + let( + pivot = arr[floor(len(arr)/2)][1][_i], + lesser = [ for (entry=arr) if (entry[1][_i] < pivot ) entry ], + equal = [ for (entry=arr) if (entry[1][_i] == pivot ) entry ], + greater = [ for (entry=arr) if (entry[1][_i] > pivot ) entry ] + ) + concat( + _sort_vectors_indexed(lesser, _i ), + _sort_vectors_indexed(equal, _i+1 ), + _sort_vectors_indexed(greater, _i ) ); + // when idx==undef, returns the sorted array // otherwise, returns the indices of the sorted array function _sort_general(arr, idx=undef) = - (len(arr)<=1) ? arr : + len(arr)<=1 ? arr : is_undef(idx) - ? _sort_scalar(arr) + ? _simple_sort(arr) +// : _lexical_sort(arr) : let( arrind=[for(k=[0:len(arr)-1], ark=[arr[k]]) [ k, [for (i=idx) ark[i]] ] ] ) - _indexed_sort(arrind); - -// given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair -// the sorting is done using compare_vals() -function _indexed_sort(arrind) = - arrind==[] ? [] : len(arrind)==1? [arrind[0][0]] : - let( pivot = arrind[floor(len(arrind)/2)][1] ) + _indexed_sort(arrind ); + +// sort simple lists with compare_vals() +function _simple_sort(arr) = + arr==[] || len(arr)==1 ? arr : let( - lesser = [ for (entry=arrind) if (compare_vals(entry[1], pivot) <0 ) entry ], - equal = [ for (entry=arrind) if (compare_vals(entry[1], pivot)==0 ) entry[0] ], - greater = [ for (entry=arrind) if (compare_vals(entry[1], pivot) >0 ) entry ] + pivot = arr[floor(len(arr)/2)], + lesser = [ for (entry=arr) if (compare_vals(entry, pivot) <0 ) entry ], + equal = [ for (entry=arr) if (compare_vals(entry, pivot)==0 ) entry ], + greater = [ for (entry=arr) if (compare_vals(entry, pivot) >0 ) entry ] ) - concat(_indexed_sort(lesser), equal, _indexed_sort(greater)); + concat( + _simple_sort(lesser), + equal, + _simple_sort(greater) + ); + + +// given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair +// it uses compare_vals() +function _lexical_sort(arr, _i=0) = + arr==[] || len(arr)==1 || _i>=len(arr[0]) ? arr : + let( + pivot = arr[floor(len(arr)/2)][_i], + lesser = [ for (entry=arr) if (compare_vals(entry[_i], pivot) <0 ) entry ], + equal = [ for (entry=arr) if (compare_vals(entry[_i], pivot)==0 ) entry ], + greater = [ for (entry=arr) if (compare_vals(entry[_i], pivot) >0 ) entry ] + ) + concat( + _lexical_sort(lesser, _i ), + _lexical_sort(equal, _i+1 ), + _lexical_sort(greater, _i ) + ); + + +// given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair +// it uses compare_vals() +function _indexed_sort(arr, _i=0) = + arr==[] ? [] : + len(arr)==1? [arr[0][0]] : + _i>=len(arr[0][1]) ? [for(ai=arr) ai[0]] : + let( + pivot = arr[floor(len(arr)/2)][1][_i], + lesser = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot) <0 ) entry ], + equal = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot)==0 ) entry ], + greater = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot) >0 ) entry ] + ) + concat( + _indexed_sort(lesser, _i ), + _indexed_sort(equal, _i+1 ), + _indexed_sort(greater, _i ) ); // returns true for valid index specifications idx in the interval [imin, imax) // note that idx can't have any value greater or EQUAL to imax +// this allows imax=INF as a bound to numerical lists function _valid_idx(idx,imin,imax) = is_undef(idx) || ( is_finite(idx) && idx>=imin && idx< imax ) || ( is_list(idx) && min(idx)>=imin && max(idx)< imax ) - || ( valid_range(idx) && idx[0]>=imin && idx[2]< imax ); + || ( ! is_list(idx) // it implicitly is a range + && (idx[1]>0 && idx[0]>=imin && idx[2]< imax) + || + (idx[0]=imin) ); // Function: sort() @@ -758,7 +830,7 @@ function _valid_idx(idx,imin,imax) = function sort(list, idx=undef) = !is_list(list) || len(list)<=1 ? list : is_def(idx) - ? assert( _valid_idx(idx,0,len(list)) , "Invalid indices.") + ? assert( _valid_idx(idx,0,len(list[0])) , "Invalid indices or out of range.") let( sarr = _sort_general(list,idx) ) [for(i=[0:len(sarr)-1]) list[sarr[i]] ] : let(size = array_dim(list)) @@ -773,6 +845,31 @@ function sort(list, idx=undef) = ) : _sort_general(list); +function sort(list, idx=undef) = + !is_list(list) || len(list)<=1 ? list : + is_vector(list) + ? assert( _valid_idx(idx,0,len(list[0])) , str("Invalid indices or out of range. ",list)) + is_def(idx) + ? sort_vector_indexed([for(i=[0:len(list)-1]) [i, [list(i)] ] ]) + : sort_scalar(list) + : is_matrix(list) + ? list==[] || list[0]==[] ? list : + assert( _valid_idx(idx,0,len(list[0])) , "Invalid indices or out of range.") + is_def(idx) + ? sort_vector_indexed([for(i=[0:len(list)-1], li=[list[i]]) [i, [for(ind=idx) li(ind)] ] ]) + : sort_vector(list) + : list==[] || list[0]==[] ? list : + let( llen = [for(li=list) !is_list(li) || is_string(li) ? 0: len(li)], + m = min(llen), + M = max(llen) + ) + M==0 ? _simple_sort(list) : + assert( m>0 && _valid_idx(idx,m-1,M) , "Invalid indices or out of range.") + is_def(idx) + ? _sort_general(list,idx) + : let( ils = _sort_general(list, [m:M]) ) + [for(i=[0:len(list)-1]) list[ils[i]] ]; + // Function: sortidx() // Description: diff --git a/math.scad b/math.scad index 888d048..e5c4625 100644 --- a/math.scad +++ b/math.scad @@ -1175,7 +1175,7 @@ function deriv3(data, h=1, closed=false) = // Description: // Multiplies two complex numbers represented by 2D vectors. function C_times(z1,z2) = - assert( is_vector(z1+z2,2), "Complex numbers should be represented by 2D vectors." ) + assert( is_matrix([z1,z2],2,2), "Complex numbers should be represented by 2D vectors" ) [ z1.x*z2.x - z1.y*z2.y, z1.x*z2.y + z1.y*z2.x ]; // Function: C_div() diff --git a/quaternions.scad b/quaternions.scad index e21801c..44b0e29 100644 --- a/quaternions.scad +++ b/quaternions.scad @@ -27,7 +27,7 @@ function _Qreal(q) = q[3]; function _Qset(v,r) = concat( v, r ); // normalizes without checking -function _Qnorm(q) = q/norm(q); +function _Qunit(q) = q/norm(q); // Function: Q_is_quat() @@ -36,7 +36,7 @@ function _Qnorm(q) = q/norm(q); // 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 Q_is_quat(q) = is_vector(q,4) ;//&& ! approx(norm(q),0) ; // Function: Quat() @@ -101,7 +101,7 @@ function QuatXYZ(a=[0,0,0]) = qy = QuatY(a[1]), qz = QuatZ(a[2]) ) - Q_Mul(qz, Q_Mul(qy, qx)); + _Qmul(qz, _Qmul(qy, qx)); // Function: Q_From_to() @@ -202,32 +202,36 @@ function Q_Sub(a, 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)") + _Qmul(a,b); + +function _Qmul(a,b) = [ a[3]*b.x + a.x*b[3] + a.y*b.z - a.z*b.y, a[3]*b.y - a.x*b.z + a.y*b[3] + a.z*b.x, a[3]*b.z + a.x*b.y - a.y*b.x + a.z*b[3], - a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z, + a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z ]; - +// [ [a[3], -a.z, a.y, a.x], +// [ a.z, a[3], -a.x, a.y], +// [-a.y, a.x, a[3], a.z], +// [-a.x, -a.y, -a.z, a[3]] ]*[b.x,b.y,b.z,b[3]] + // Function: Q_Cumulative() // Usage: -// Q_Cumulative(v); +// Q_Cumulative(ql); // 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], select(_acc,-1))] - ) - ); +function Q_Cumulative(ql) = + assert( is_matrix(ql,undef,4) && len(ql)>0, "Invalid list of quaternions." ) + [for( i = 0, q = ql[0]; + i<=len(ql); + i = i+1, q = (i==len(ql))? 0: _Qmul(q,ql[i]) ) + q ]; // Function: Q_Dot() @@ -252,7 +256,7 @@ function Q_Neg(q) = // Q_Conj(q) // Description: Returns the conjugate of quaternion `q`. function Q_Conj(q) = - assert( Q_is_quat(q), "Invalid quaternion" ) + assert( Q_is_quat(q), str("Invalid quaternion",q) ) [-q.x, -q.y, -q.z, q[3]]; @@ -262,7 +266,7 @@ function Q_Conj(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) ) +// let(q = q/norm(q) ) [-q.x, -q.y, -q.z, q[3]]; @@ -283,7 +287,9 @@ function Q_Norm(q) = // Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1. function Q_Normalize(q) = assert( Q_is_quat(q) , "Invalid quaternion" ) - q/norm(q); + approx(_Qvec(q), [0,0,0]) + ? Q_Ident() + : q/norm(q); // Function: Q_Dist() @@ -318,31 +324,32 @@ function Q_Dist(q1, q2) = // Qrot(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(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) - let( - _dot = q1*q2, - q1 = q1/norm(q1), - q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), - dot = abs(_dot) - ) - ! is_finite(u) ? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] : - Q_Slerp(q1, q2, u, dot) - : _dot>0.9995 - ? _Qnorm(q1 + u*(q2-q1)) - : let( theta = u*acos(_dot), - q3 = _Qnorm(q2 - _dot*q1) - ) - _Qnorm(q1*cos(theta) + q3*sin(theta)); + assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(s)") + assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) + let( + _dot = q1*q2, + q1 = q1/norm(q1), + q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), + dot = abs(_dot), + q3 = dot>0.9995? q2: _Qunit(q2 - dot*q1) + ) + ! is_num(u) + ? [for (uu=u) _Qslerp(q1, q3, uu, dot)] + : _Qslerp(q1, q3, u, dot); +function _Qslerp(q1, q2, u, dot) = + dot>0.9995 + ? _Qunit(q1 + u*(q2-q1)) + : let( theta = u*acos(dot) ) + _Qunit(q1*cos(theta) + q2*sin(theta)); + -// Function: Q_Matrix3() +// Function: Q_to_matrix3() // Usage: -// Q_Matrix3(q); +// Q_to_matrix3(q); // Description: // Returns the 3x3 rotation matrix for the given normalized quaternion q. -function Q_Matrix3(q) = +function Q_to_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]], @@ -351,12 +358,12 @@ function Q_Matrix3(q) = ]; -// Function: Q_Matrix4() +// Function: Q_to_matrix4() // Usage: -// Q_Matrix4(q); +// Q_to_matrix4(q); // Description: // Returns the 4x4 rotation matrix for the given normalized quaternion q. -function Q_Matrix4(q) = +function Q_to_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], @@ -366,6 +373,35 @@ function Q_Matrix4(q) = ]; +// Function: Q_from_matrix() +// Usage: +// Q_from_matrix(M) +// Description: +// Returns a normalized quaternion corresponding to the rotation matrix M. +// M may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. +// The last row and last column of M are ignored for 4x4 matrices. +// It doesn't check whether M is in fact a rotation matrix. +// If M is not a rotation, the returned quaternion is unpredictable. +// +// based on https://en.wikipedia.org/wiki/Rotation_matrix +// +function Q_from_matrix(M) = + assert( is_matrix(M) && (len(M)==3 || len(M)==4) && (len(M[0])==3 || len(M[0])==4), + "The matrix should be 3x3 or 4x4") + let( tr = M[0][0]+M[1][1]+M[2][2] ) // M trace + tr>0 + ? let( r = sqrt(1+tr), s = -1/r/2 ) + _Qunit( _Qset([ M[1][2]-M[2][1], M[2][0]-M[0][2], M[0][1]-M[1][0] ]*s, r/2 ) ) + : let( + i = max_index([ M[0][0], M[1][1], M[2][2] ]), + r = sqrt(1 + 2*M[i][i] -M[0][0] -M[1][1] -M[2][2] ), + s = 1/r/2 + ) + i==0 ? _Qunit( _Qset( [ r/2, s*(M[1][0]+M[0][1]), s*(M[0][2]+M[2][0]) ], s*(M[2][1]-M[1][2])) ): + i==1 ? _Qunit( _Qset( [ s*(M[1][0]+M[0][1]), r/2, s*(M[2][1]+M[1][2]) ], s*(M[0][2]-M[2][0])) ) + : _Qunit( _Qset( [ s*(M[2][0]+M[0][2]), s*(M[1][2]+M[2][1]), r/2 ], s*(M[1][0]-M[0][1])) ) ; + + // Function: Q_Axis() // Usage: // Q_Axis(q) @@ -418,15 +454,15 @@ function Q_Angle(q1,q2) = // 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)) { + multmatrix(Q_to_matrix4(q)) { children(); } } function Qrot(q,p) = - is_undef(p)? Q_Matrix4(q) : + is_undef(p)? Q_to_matrix4(q) : is_vector(p)? Qrot(q,[p])[0] : - apply(Q_Matrix4(q), p); + apply(Q_to_matrix4(q), p); // Module: Qrot_copies() @@ -446,29 +482,6 @@ function Qrot(q,p) = module Qrot_copies(quats) for (q=quats) Qrot(q) children(); -// Function: Q_Rotation() -// Usage: -// Q_Rotation(R) -// Description: -// Returns a normalized quaternion corresponding to the rotation matrix R. -// R may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. -// The last row and last column of R are ignored for 4x4 matrices. -// It doesn't check whether R is in fact a rotation matrix. -// If R is not a rotation, the returned quaternion is an unpredictable quaternion . -function Q_Rotation(R) = - assert( is_matrix(R,3,3) || is_matrix(R,4,4) , - "Matrix is neither 3x3 nor 4x4") - let( tr = R[0][0]+R[1][1]+R[2][2] ) // R trace - tr>0 - ? let( r = 1+tr ) - _Qnorm( _Qset([ R[1][2]-R[2][1], R[2][0]-R[0][2], R[0][1]-R[1][0] ], -r ) ) - : let( i = max_index([ R[0][0], R[1][1], R[2][2] ]), - r = 1 + 2*R[i][i] -R[0][0] -R[1][1] -R[2][2] ) - i==0 ? _Qnorm( _Qset( [ 4*r, (R[1][0]+R[0][1]), (R[0][2]+R[2][0]) ], (R[2][1]-R[1][2])) ): - i==1 ? _Qnorm( _Qset( [ (R[1][0]+R[0][1]), 4*r, (R[2][1]+R[1][2]) ], (R[0][2]-R[2][0])) ): - _Qnorm( _Qset( [ (R[2][0]+R[0][2]), (R[1][2]+R[2][1]), 4*r ], (R[1][0]-R[0][1])) ) ; - - // Function&Module: Q_Rotation_path(q1, n, [q2]) // Usage: As a function // path = Q_Rotation_path(q1, n, q2); @@ -518,11 +531,11 @@ function Q_Rotation_path(q1, n=1, q2) = 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] + ? [for( i=0, dR=Q_to_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]; + dq = Q_pow( _Qmul( q2, Q_Inverse(q1) ), 1/n ), + dR = Q_to_matrix4(dq) ) + [for( i=0, R=Q_to_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)) @@ -565,8 +578,8 @@ function Q_Nlerp(q1,q2,u) = 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 ) ]; + ? _Qunit((1-u)*q1 + u*q2 ) + : [for (ui=u) _Qunit((1-ui)*q1 + ui*q2 ) ]; // Function: Q_Squad() @@ -616,6 +629,17 @@ function Q_Squad(q1,q2,q3,q4,u) = : [for(ui=u) Q_Slerp( Q_Slerp(q1,q4,ui), Q_Slerp(q2,q3,ui), 2*ui*(1-ui) ) ]; +function Q_Scubic(q1,q2,q3,q4,u) = + assert(is_finite(u) || is_range(u) || is_vector(u) , + "Invalid interpolation coefficient(s)" ) + is_num(u) + ? let( q12 = Q_Slerp(q1,q2,u), + q23 = Q_Slerp(q2,q3,u), + q34 = Q_Slerp(q3,q4,u) ) + Q_Slerp(Q_Slerp(q12,q23,u),Q_Slerp(q23,q34,u),u) + : [for(ui=u) Q_Scubic( q1,q2,q3,q4,ui) ]; + + // Function: Q_exp() // Usage: // q2 = Q_exp(q); diff --git a/tests/test_all.scad b/tests/test_all.scad new file mode 100644 index 0000000..fe6512b --- /dev/null +++ b/tests/test_all.scad @@ -0,0 +1,28 @@ +//include +include +include +include +include +include +include +include +include +include +include +include +include +include +//include +include +include +include +include +include +include +include +include +include +include +include + + diff --git a/tests/test_quaternions.scad b/tests/test_quaternions.scad index b1e5130..af19408 100644 --- a/tests/test_quaternions.scad +++ b/tests/test_quaternions.scad @@ -1,6 +1,80 @@ include <../std.scad> include <../strings.scad> +function plane_fit(points) = + assert( is_matrix(points,undef,3) , "Improper point list." ) + len(points)< 2 ? [] : + len(points)==3 ? plane3pt(points[0],points[1],points[2]) : + let( + A = [for(pi=points) concat(pi,-1) ], + qr = qr_factor(A), + R = select(qr[1],0,3), +x=[for(ri=R) echo(R=ri)0], +//y=[for(qi=qr[0]) echo(Q=qi)0], + s = back_substitute + ( R, + sum([for(qi=qr[0])[for(j=[0:3]) qi[j] ] ]) + ) +//, s0 = echo(ls=linear_solve(A,[for(p=points) 0])) +,w=s==[]? echo("s == []")0:echo(s=s)0 + ) + s==[] + ? // points are collinear + [] + : // plane through the origin? + approx(norm([s.x, s.y, s.z]), 0) + ? let( + k = max_index([for(i=[0:2]) abs(s[i]) ]), + A = [[1,0,0], for(pi=points) pi ], + b = [1, for(i=[1:len(points)]) 0], + s = linear_solve(A,b) +, y=echo(s2=s) + ) + s==[]? []: + concat(s, 0)/norm(s) + : s/norm([s.x, s.y, s.z]) ; + +function plane_fit(points) = + assert( is_matrix(points,undef,3) , "Improper point list." ) + len(points)< 2 ? [] : + len(points)==3 ? plane3pt(points[0],points[1],points[2]) : + let( + A = [for(pi=points) concat(pi,-1) ], + B = [ for(pi=points) [0,1] ], + sB = transpose(linear_solve(A,B)), + s = sB==[] ? [] + : norm(sB[1])N/2? 10*[1,1,1]: rands(-1,1,3) ]; +pf = plane_fit(pts); +echo(pf); +//pm = concat(sum(pts)/len(pts), -1); +//echo(pmfit=pm*pf); function rec_cmp(a,b,eps=1e-9) = typeof(a)!=typeof(b)? false : @@ -8,7 +82,18 @@ function rec_cmp(a,b,eps=1e-9) = is_list(a)? len(a)==len(b) && all([for (i=idx(a)) rec_cmp(a[i],b[i],eps=eps)]) : a == b; -function Qstandard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q; +function standardize(v) = + v==[] + ? [] + : sign(first_nonzero(v))*v; + +function first_nonzero(v) = + v==[] ? 0 + : is_num(v) ? v + : [for(vi=v) if(!is_list(vi) && ! approx(vi,0)) vi + else if(is_list(vi)) first_nonzero(vi), 0 ][0]; + +module assert_std(vc,ve) { assert_approx(standardize(vc),standardize(ve)); } module verify_f(actual,expected) { if (!rec_cmp(actual,expected)) { @@ -25,7 +110,7 @@ module verify_f(actual,expected) { module test_is_quat() { verify_f(Q_is_quat([0]),false); - verify_f(Q_is_quat([0,0,0,0]),false); + verify_f(Q_is_quat([0,0,0,0]),true); verify_f(Q_is_quat([1,0,2,0]),true); verify_f(Q_is_quat([1,0,2,0,0]),false); } @@ -104,7 +189,7 @@ test_QuatXYZ(); module test_Q_From_to() { verify_f(Q_Mul(Q_From_to([1,2,3], [4,5,2]),Q_From_to([4,5,2], [1,2,3])), Q_Ident()); - verify_f(Q_Matrix4(Q_From_to([1,2,3], [4,5,2])), rot(from=[1,2,3],to=[4,5,2])); + verify_f(Q_to_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])); } @@ -200,7 +285,7 @@ test_Q_Mul(); 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]]); } -test_Q_Cumulative(); +*test_Q_Cumulative(); module test_Q_Dot() { @@ -219,18 +304,18 @@ test_Q_Neg(); module test_Q_Conj() { + ang = rands(0,360,3); 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]); + verify_f(Q_Mul(Q_Conj(QuatXYZ(ang)),QuatXYZ(ang)),Q_Ident()); } test_Q_Conj(); module test_Q_Inverse() { - - verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]/sqrt(2)); - verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]/sqrt(2)); - verify_f(Q_Inverse(QuatXYZ([23,45,67])),Q_Conj(QuatXYZ([23,45,67]))); + verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]); + verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]); verify_f(Q_Mul(Q_Inverse(QuatXYZ([23,45,67])),QuatXYZ([23,45,67])),Q_Ident()); } test_Q_Inverse(); @@ -247,7 +332,7 @@ test_Q_Norm(); 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]); +// verify_f(Q_Normalize(QuatXYZ([23,45,67])),[-0.0533818345, 0.4143703268, 0.4360652669, 0.7970537592]); } test_Q_Normalize(); @@ -260,28 +345,47 @@ test_Q_Dist(); 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)); + u = rands(0,1,1)[0]; + ul = rands(0,1,5); + ul2 = [1,1,1,1,1]-ul; + a1 = rands(0,360,1)[0]; + a2 = rands(0,360,1)[0]; + a3 = rands(0,360,1)[0]; + verify_f(standardize(Q_Slerp(QuatX(a1),QuatY(a2),0.0)), standardize(QuatX(a1))); + verify_f(standardize(Q_Slerp(QuatX(45),QuatY(30),0.5)), + [0.1967063121, 0.1330377423, 0, 0.9713946602]); + verify_f(standardize(Q_Slerp(QuatX(a1),QuatY(a2),1.0)), standardize(QuatY(a2))); + verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),-QuatY(a2),u)), + standardize(Q_Slerp(QuatXYZ([a1,a2,0]), QuatY(a2),u))); + verify_f(standardize(Q_Slerp(QuatX(a1),QuatX(a1),u)), standardize(QuatX(a1))); + verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),QuatXYZ([a2,0,a1]),u)), + standardize(Q_Slerp(QuatXYZ([a2,0,a1]),QuatXYZ([a1,a2,0]),1-u))); + verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),QuatXYZ([a2,0,a1]),ul)), + standardize(Q_Slerp(QuatXYZ([a2,0,a1]),QuatXYZ([a1,a2,0]),ul2))); } test_Q_Slerp(); -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_to_matrix3() { + rotZ_37 = rot(37); + rotZ_37_3 = [for(i=[0:2]) [for(j=[0:2]) rotZ_37[i][j] ] ]; + angs = [12,-49,40]; + rot4 = rot(angs); + rot3 = [for(i=[0:2]) [for(j=[0:2]) rot4[i][j] ] ]; + verify_f(Q_to_matrix3(QuatZ(37)),rotZ_37_3); + verify_f(Q_to_matrix3(QuatXYZ(angs)),rot3); } -test_Q_Matrix3(); +test_Q_to_matrix3(); -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_to_matrix4() { + verify_f(Q_to_matrix4(QuatZ(37)),rot(37)); + verify_f(Q_to_matrix4(QuatZ(-49)),rot(-49)); + verify_f(Q_to_matrix4(QuatX(37)),rot([37,0,0])); + verify_f(Q_to_matrix4(QuatY(37)),rot([0,37,0])); + verify_f(Q_to_matrix4(QuatXYZ([12,34,56])),rot([12,34,56])); } -test_Q_Matrix4(); +test_Q_to_matrix4(); module test_Q_Axis() { @@ -321,23 +425,23 @@ module test_Qrot() { test_Qrot(); -module test_Q_Rotation() { - verify_f(Qstandard(Q_Rotation(Q_Matrix3(Quat([12,34,56],33)))),Qstandard(Quat([12,34,56],33))); - verify_f(Q_Matrix3(Q_Rotation(Q_Matrix3(QuatXYZ([12,34,56])))), - Q_Matrix3(QuatXYZ([12,34,56]))); +module test_Q_from_matrix() { + verify_f(standardize(Q_from_matrix(Q_to_matrix3(Quat([12,34,56],33)))),standardize(Quat([12,34,56],33))); + verify_f(Q_to_matrix3(Q_from_matrix(Q_to_matrix3(QuatXYZ([12,34,56])))), + Q_to_matrix3(QuatXYZ([12,34,56]))); } -test_Q_Rotation(); +test_Q_from_matrix(); 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), 5, QuatY(13.5))[0] , Q_to_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)[3] , xrot(11+(55-11)*3/4)); verify_f(Q_Rotation_path(QuatX(11), 5)[4] , xrot(55)); } @@ -347,48 +451,54 @@ 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_Rotation_path(QuatX(135), 16, QuatY(13.5))[8] , + Q_to_matrix4(Q_Nlerp(QuatX(135), QuatY(13.5),0.5))); verify_f(Q_Nlerp(QuatX(45),QuatY(30),1.0),QuatY(30)); } test_Q_Nlerp(); module test_Q_Squad() { - verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(90),QuatY(30),0.0),QuatX(45)); + u = rands(0,1,5); + su = [1,1,1,1,1]-u; + verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(32),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), + 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)); + verify_f( Q_Squad(QuatY(10),QuatZ(20),QuatX(32),QuatXYZ([210,120,30]),u[0] ), + Q_Squad(QuatXYZ([210,120,30]),QuatX(32),QuatZ(20),QuatY(10),1-u[0] ) ); + verify_f( Q_Squad(QuatY(10),QuatZ(20),QuatX(32),QuatXYZ([210,120,30]),u ), + Q_Squad(QuatXYZ([210,120,30]),QuatX(32),QuatZ(20),QuatY(10),su ) ); } test_Q_Squad(); module test_Q_exp() { + q=QuatXYZ(rands(0,360,3)); 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)))); + verify_f(Q_exp(Q_ln([1,2,3,4])), [1,2,3,4]); + verify_f(Q_exp(Q_ln(q)), q); + verify_f(Q_exp(q+Q_Inverse(q)),Q_Mul(Q_exp(q),Q_exp(Q_Inverse(q)))); } test_Q_exp(); module test_Q_ln() { + q = QuatXYZ(rands(0,360,3)); 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]); + verify_f(Q_ln(Q_exp(q)), q); + verify_f(Q_ln(q)+Q_ln(Q_Conj(q)), [0,0,0,0]); } test_Q_ln(); module test_Q_pow() { - q = Quat([1,2,3],77); + q = QuatXYZ(rands(0,360,3)); verify_f(Q_pow(q,1), q); verify_f(Q_pow(q,0), Q_Ident()); verify_f(Q_pow(q,-1), Q_Inverse(q)); From 069432021b420aab2ce91ce1bfaf3cf73c8cef28 Mon Sep 17 00:00:00 2001 From: RonaldoCMP Date: Wed, 26 Aug 2020 14:09:32 +0100 Subject: [PATCH 4/5] Revert "Revert "Revert "Correction of C_times validation""" This reverts commit e8499baba185848ff6f6f1745181a33573ee5c58. --- arrays.scad | 125 +++-------------------- math.scad | 2 +- quaternions.scad | 170 ++++++++++++++----------------- tests/test_all.scad | 28 ------ tests/test_quaternions.scad | 196 ++++++++---------------------------- 5 files changed, 131 insertions(+), 390 deletions(-) delete mode 100644 tests/test_all.scad diff --git a/arrays.scad b/arrays.scad index 3ae7a88..835fd65 100644 --- a/arrays.scad +++ b/arrays.scad @@ -708,108 +708,36 @@ function _sort_vectors4(arr) = y ] ) concat( _sort_vectors4(lesser), equal, _sort_vectors4(greater) ); -// sort a list of vectors -function _sort_vectors(arr, _i=0) = - len(arr)<=1 || _i>=len(arr[0]) ? arr : - let( - pivot = arr[floor(len(arr)/2)][_i], - lesser = [ for (entry=arr) if (entry[_i] < pivot ) entry ], - equal = [ for (entry=arr) if (entry[_i] == pivot ) entry ], - greater = [ for (entry=arr) if (entry[_i] > pivot ) entry ] - ) - concat( - _sort_vectors(lesser, _i ), - _sort_vectors(equal, _i+1 ), - _sort_vectors(greater, _i ) ); - -// given pairs of an index and a vector, return the list of indices of the list sorted by the vectors -function _sort_vectors_indexed(arr, _i=0) = - arr==[] ? [] : - len(arr)==1 || _i>=len(arr[0][1]) ? [for(ai=arr) ai[0]] : - let( - pivot = arr[floor(len(arr)/2)][1][_i], - lesser = [ for (entry=arr) if (entry[1][_i] < pivot ) entry ], - equal = [ for (entry=arr) if (entry[1][_i] == pivot ) entry ], - greater = [ for (entry=arr) if (entry[1][_i] > pivot ) entry ] - ) - concat( - _sort_vectors_indexed(lesser, _i ), - _sort_vectors_indexed(equal, _i+1 ), - _sort_vectors_indexed(greater, _i ) ); - // when idx==undef, returns the sorted array // otherwise, returns the indices of the sorted array function _sort_general(arr, idx=undef) = - len(arr)<=1 ? arr : + (len(arr)<=1) ? arr : is_undef(idx) - ? _simple_sort(arr) -// : _lexical_sort(arr) + ? _sort_scalar(arr) : let( arrind=[for(k=[0:len(arr)-1], ark=[arr[k]]) [ k, [for (i=idx) ark[i]] ] ] ) - _indexed_sort(arrind ); - -// sort simple lists with compare_vals() -function _simple_sort(arr) = - arr==[] || len(arr)==1 ? arr : - let( - pivot = arr[floor(len(arr)/2)], - lesser = [ for (entry=arr) if (compare_vals(entry, pivot) <0 ) entry ], - equal = [ for (entry=arr) if (compare_vals(entry, pivot)==0 ) entry ], - greater = [ for (entry=arr) if (compare_vals(entry, pivot) >0 ) entry ] - ) - concat( - _simple_sort(lesser), - equal, - _simple_sort(greater) - ); - - + _indexed_sort(arrind); + // given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair -// it uses compare_vals() -function _lexical_sort(arr, _i=0) = - arr==[] || len(arr)==1 || _i>=len(arr[0]) ? arr : +// the sorting is done using compare_vals() +function _indexed_sort(arrind) = + arrind==[] ? [] : len(arrind)==1? [arrind[0][0]] : + let( pivot = arrind[floor(len(arrind)/2)][1] ) let( - pivot = arr[floor(len(arr)/2)][_i], - lesser = [ for (entry=arr) if (compare_vals(entry[_i], pivot) <0 ) entry ], - equal = [ for (entry=arr) if (compare_vals(entry[_i], pivot)==0 ) entry ], - greater = [ for (entry=arr) if (compare_vals(entry[_i], pivot) >0 ) entry ] + lesser = [ for (entry=arrind) if (compare_vals(entry[1], pivot) <0 ) entry ], + equal = [ for (entry=arrind) if (compare_vals(entry[1], pivot)==0 ) entry[0] ], + greater = [ for (entry=arrind) if (compare_vals(entry[1], pivot) >0 ) entry ] ) - concat( - _lexical_sort(lesser, _i ), - _lexical_sort(equal, _i+1 ), - _lexical_sort(greater, _i ) - ); - - -// given a list of pairs, return the first element of each pair of the list sorted by the second element of the pair -// it uses compare_vals() -function _indexed_sort(arr, _i=0) = - arr==[] ? [] : - len(arr)==1? [arr[0][0]] : - _i>=len(arr[0][1]) ? [for(ai=arr) ai[0]] : - let( - pivot = arr[floor(len(arr)/2)][1][_i], - lesser = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot) <0 ) entry ], - equal = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot)==0 ) entry ], - greater = [ for (entry=arr) if (compare_vals(entry[1][_i], pivot) >0 ) entry ] - ) - concat( - _indexed_sort(lesser, _i ), - _indexed_sort(equal, _i+1 ), - _indexed_sort(greater, _i ) ); + concat(_indexed_sort(lesser), equal, _indexed_sort(greater)); // returns true for valid index specifications idx in the interval [imin, imax) // note that idx can't have any value greater or EQUAL to imax -// this allows imax=INF as a bound to numerical lists function _valid_idx(idx,imin,imax) = is_undef(idx) || ( is_finite(idx) && idx>=imin && idx< imax ) || ( is_list(idx) && min(idx)>=imin && max(idx)< imax ) - || ( ! is_list(idx) // it implicitly is a range - && (idx[1]>0 && idx[0]>=imin && idx[2]< imax) - || - (idx[0]=imin) ); + || ( valid_range(idx) && idx[0]>=imin && idx[2]< imax ); // Function: sort() @@ -830,7 +758,7 @@ function _valid_idx(idx,imin,imax) = function sort(list, idx=undef) = !is_list(list) || len(list)<=1 ? list : is_def(idx) - ? assert( _valid_idx(idx,0,len(list[0])) , "Invalid indices or out of range.") + ? assert( _valid_idx(idx,0,len(list)) , "Invalid indices.") let( sarr = _sort_general(list,idx) ) [for(i=[0:len(sarr)-1]) list[sarr[i]] ] : let(size = array_dim(list)) @@ -845,31 +773,6 @@ function sort(list, idx=undef) = ) : _sort_general(list); -function sort(list, idx=undef) = - !is_list(list) || len(list)<=1 ? list : - is_vector(list) - ? assert( _valid_idx(idx,0,len(list[0])) , str("Invalid indices or out of range. ",list)) - is_def(idx) - ? sort_vector_indexed([for(i=[0:len(list)-1]) [i, [list(i)] ] ]) - : sort_scalar(list) - : is_matrix(list) - ? list==[] || list[0]==[] ? list : - assert( _valid_idx(idx,0,len(list[0])) , "Invalid indices or out of range.") - is_def(idx) - ? sort_vector_indexed([for(i=[0:len(list)-1], li=[list[i]]) [i, [for(ind=idx) li(ind)] ] ]) - : sort_vector(list) - : list==[] || list[0]==[] ? list : - let( llen = [for(li=list) !is_list(li) || is_string(li) ? 0: len(li)], - m = min(llen), - M = max(llen) - ) - M==0 ? _simple_sort(list) : - assert( m>0 && _valid_idx(idx,m-1,M) , "Invalid indices or out of range.") - is_def(idx) - ? _sort_general(list,idx) - : let( ils = _sort_general(list, [m:M]) ) - [for(i=[0:len(list)-1]) list[ils[i]] ]; - // Function: sortidx() // Description: diff --git a/math.scad b/math.scad index e5c4625..888d048 100644 --- a/math.scad +++ b/math.scad @@ -1175,7 +1175,7 @@ function deriv3(data, h=1, closed=false) = // Description: // Multiplies two complex numbers represented by 2D vectors. function C_times(z1,z2) = - assert( is_matrix([z1,z2],2,2), "Complex numbers should be represented by 2D vectors" ) + assert( is_vector(z1+z2,2), "Complex numbers should be represented by 2D vectors." ) [ z1.x*z2.x - z1.y*z2.y, z1.x*z2.y + z1.y*z2.x ]; // Function: C_div() diff --git a/quaternions.scad b/quaternions.scad index 44b0e29..e21801c 100644 --- a/quaternions.scad +++ b/quaternions.scad @@ -27,7 +27,7 @@ function _Qreal(q) = q[3]; function _Qset(v,r) = concat( v, r ); // normalizes without checking -function _Qunit(q) = q/norm(q); +function _Qnorm(q) = q/norm(q); // Function: Q_is_quat() @@ -36,7 +36,7 @@ function _Qunit(q) = q/norm(q); // 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 Q_is_quat(q) = is_vector(q,4) && ! approx(norm(q),0) ; // Function: Quat() @@ -101,7 +101,7 @@ function QuatXYZ(a=[0,0,0]) = qy = QuatY(a[1]), qz = QuatZ(a[2]) ) - _Qmul(qz, _Qmul(qy, qx)); + Q_Mul(qz, Q_Mul(qy, qx)); // Function: Q_From_to() @@ -202,36 +202,32 @@ function Q_Sub(a, 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)") - _Qmul(a,b); - -function _Qmul(a,b) = [ a[3]*b.x + a.x*b[3] + a.y*b.z - a.z*b.y, a[3]*b.y - a.x*b.z + a.y*b[3] + a.z*b.x, a[3]*b.z + a.x*b.y - a.y*b.x + a.z*b[3], - a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z + a[3]*b[3] - a.x*b.x - a.y*b.y - a.z*b.z, ]; -// [ [a[3], -a.z, a.y, a.x], -// [ a.z, a[3], -a.x, a.y], -// [-a.y, a.x, a[3], a.z], -// [-a.x, -a.y, -a.z, a[3]] ]*[b.x,b.y,b.z,b[3]] - + // Function: Q_Cumulative() // Usage: -// Q_Cumulative(ql); +// 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(ql) = - assert( is_matrix(ql,undef,4) && len(ql)>0, "Invalid list of quaternions." ) - [for( i = 0, q = ql[0]; - i<=len(ql); - i = i+1, q = (i==len(ql))? 0: _Qmul(q,ql[i]) ) - q ]; +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], select(_acc,-1))] + ) + ); // Function: Q_Dot() @@ -256,7 +252,7 @@ function Q_Neg(q) = // Q_Conj(q) // Description: Returns the conjugate of quaternion `q`. function Q_Conj(q) = - assert( Q_is_quat(q), str("Invalid quaternion",q) ) + assert( Q_is_quat(q), "Invalid quaternion" ) [-q.x, -q.y, -q.z, q[3]]; @@ -266,7 +262,7 @@ function Q_Conj(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 = q/norm(q) ) + let(q = _Qnorm(q) ) [-q.x, -q.y, -q.z, q[3]]; @@ -287,9 +283,7 @@ function Q_Norm(q) = // Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1. function Q_Normalize(q) = assert( Q_is_quat(q) , "Invalid quaternion" ) - approx(_Qvec(q), [0,0,0]) - ? Q_Ident() - : q/norm(q); + q/norm(q); // Function: Q_Dist() @@ -324,32 +318,31 @@ function Q_Dist(q1, q2) = // Qrot(q) right(80) cube([10,10,1]); // #sphere(r=80); function Q_Slerp(q1, q2, u, _dot) = - assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(s)") - assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) - let( - _dot = q1*q2, - q1 = q1/norm(q1), - q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), - dot = abs(_dot), - q3 = dot>0.9995? q2: _Qunit(q2 - dot*q1) - ) - ! is_num(u) - ? [for (uu=u) _Qslerp(q1, q3, uu, dot)] - : _Qslerp(q1, q3, u, dot); + is_undef(_dot) + ? assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(s)") + assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" ) + let( + _dot = q1*q2, + q1 = q1/norm(q1), + q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2), + dot = abs(_dot) + ) + ! is_finite(u) ? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] : + Q_Slerp(q1, q2, u, dot) + : _dot>0.9995 + ? _Qnorm(q1 + u*(q2-q1)) + : let( theta = u*acos(_dot), + q3 = _Qnorm(q2 - _dot*q1) + ) + _Qnorm(q1*cos(theta) + q3*sin(theta)); -function _Qslerp(q1, q2, u, dot) = - dot>0.9995 - ? _Qunit(q1 + u*(q2-q1)) - : let( theta = u*acos(dot) ) - _Qunit(q1*cos(theta) + q2*sin(theta)); - -// Function: Q_to_matrix3() +// Function: Q_Matrix3() // Usage: -// Q_to_matrix3(q); +// Q_Matrix3(q); // Description: // Returns the 3x3 rotation matrix for the given normalized quaternion q. -function Q_to_matrix3(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]], @@ -358,12 +351,12 @@ function Q_to_matrix3(q) = ]; -// Function: Q_to_matrix4() +// Function: Q_Matrix4() // Usage: -// Q_to_matrix4(q); +// Q_Matrix4(q); // Description: // Returns the 4x4 rotation matrix for the given normalized quaternion q. -function Q_to_matrix4(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], @@ -373,35 +366,6 @@ function Q_to_matrix4(q) = ]; -// Function: Q_from_matrix() -// Usage: -// Q_from_matrix(M) -// Description: -// Returns a normalized quaternion corresponding to the rotation matrix M. -// M may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. -// The last row and last column of M are ignored for 4x4 matrices. -// It doesn't check whether M is in fact a rotation matrix. -// If M is not a rotation, the returned quaternion is unpredictable. -// -// based on https://en.wikipedia.org/wiki/Rotation_matrix -// -function Q_from_matrix(M) = - assert( is_matrix(M) && (len(M)==3 || len(M)==4) && (len(M[0])==3 || len(M[0])==4), - "The matrix should be 3x3 or 4x4") - let( tr = M[0][0]+M[1][1]+M[2][2] ) // M trace - tr>0 - ? let( r = sqrt(1+tr), s = -1/r/2 ) - _Qunit( _Qset([ M[1][2]-M[2][1], M[2][0]-M[0][2], M[0][1]-M[1][0] ]*s, r/2 ) ) - : let( - i = max_index([ M[0][0], M[1][1], M[2][2] ]), - r = sqrt(1 + 2*M[i][i] -M[0][0] -M[1][1] -M[2][2] ), - s = 1/r/2 - ) - i==0 ? _Qunit( _Qset( [ r/2, s*(M[1][0]+M[0][1]), s*(M[0][2]+M[2][0]) ], s*(M[2][1]-M[1][2])) ): - i==1 ? _Qunit( _Qset( [ s*(M[1][0]+M[0][1]), r/2, s*(M[2][1]+M[1][2]) ], s*(M[0][2]-M[2][0])) ) - : _Qunit( _Qset( [ s*(M[2][0]+M[0][2]), s*(M[1][2]+M[2][1]), r/2 ], s*(M[1][0]-M[0][1])) ) ; - - // Function: Q_Axis() // Usage: // Q_Axis(q) @@ -454,15 +418,15 @@ function Q_Angle(q1,q2) = // q = QuatXYZ([45,35,10]); // pts = Qrot(q, p=[[2,3,4], [4,5,6], [9,2,3]]); module Qrot(q) { - multmatrix(Q_to_matrix4(q)) { + multmatrix(Q_Matrix4(q)) { children(); } } function Qrot(q,p) = - is_undef(p)? Q_to_matrix4(q) : + is_undef(p)? Q_Matrix4(q) : is_vector(p)? Qrot(q,[p])[0] : - apply(Q_to_matrix4(q), p); + apply(Q_Matrix4(q), p); // Module: Qrot_copies() @@ -482,6 +446,29 @@ function Qrot(q,p) = module Qrot_copies(quats) for (q=quats) Qrot(q) children(); +// Function: Q_Rotation() +// Usage: +// Q_Rotation(R) +// Description: +// Returns a normalized quaternion corresponding to the rotation matrix R. +// R may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix. +// The last row and last column of R are ignored for 4x4 matrices. +// It doesn't check whether R is in fact a rotation matrix. +// If R is not a rotation, the returned quaternion is an unpredictable quaternion . +function Q_Rotation(R) = + assert( is_matrix(R,3,3) || is_matrix(R,4,4) , + "Matrix is neither 3x3 nor 4x4") + let( tr = R[0][0]+R[1][1]+R[2][2] ) // R trace + tr>0 + ? let( r = 1+tr ) + _Qnorm( _Qset([ R[1][2]-R[2][1], R[2][0]-R[0][2], R[0][1]-R[1][0] ], -r ) ) + : let( i = max_index([ R[0][0], R[1][1], R[2][2] ]), + r = 1 + 2*R[i][i] -R[0][0] -R[1][1] -R[2][2] ) + i==0 ? _Qnorm( _Qset( [ 4*r, (R[1][0]+R[0][1]), (R[0][2]+R[2][0]) ], (R[2][1]-R[1][2])) ): + i==1 ? _Qnorm( _Qset( [ (R[1][0]+R[0][1]), 4*r, (R[2][1]+R[1][2]) ], (R[0][2]-R[2][0])) ): + _Qnorm( _Qset( [ (R[2][0]+R[0][2]), (R[1][2]+R[2][1]), 4*r ], (R[1][0]-R[0][1])) ) ; + + // Function&Module: Q_Rotation_path(q1, n, [q2]) // Usage: As a function // path = Q_Rotation_path(q1, n, q2); @@ -531,11 +518,11 @@ function Q_Rotation_path(q1, n=1, q2) = 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_to_matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R] + ? [for( i=0, dR=Q_Matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R] : let( q2 = Q_Normalize( q1*q2<0 ? -q2: q2 ), - dq = Q_pow( _Qmul( q2, Q_Inverse(q1) ), 1/n ), - dR = Q_to_matrix4(dq) ) - [for( i=0, R=Q_to_matrix4(q1); i<=n; i=i+1, R=dR*R ) R]; + 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)) @@ -578,8 +565,8 @@ function Q_Nlerp(q1,q2,u) = let( q1 = Q_Normalize(q1), q2 = Q_Normalize(q2) ) is_num(u) - ? _Qunit((1-u)*q1 + u*q2 ) - : [for (ui=u) _Qunit((1-ui)*q1 + ui*q2 ) ]; + ? _Qnorm((1-u)*q1 + u*q2 ) + : [for (ui=u) _Qnorm((1-ui)*q1 + ui*q2 ) ]; // Function: Q_Squad() @@ -629,17 +616,6 @@ function Q_Squad(q1,q2,q3,q4,u) = : [for(ui=u) Q_Slerp( Q_Slerp(q1,q4,ui), Q_Slerp(q2,q3,ui), 2*ui*(1-ui) ) ]; -function Q_Scubic(q1,q2,q3,q4,u) = - assert(is_finite(u) || is_range(u) || is_vector(u) , - "Invalid interpolation coefficient(s)" ) - is_num(u) - ? let( q12 = Q_Slerp(q1,q2,u), - q23 = Q_Slerp(q2,q3,u), - q34 = Q_Slerp(q3,q4,u) ) - Q_Slerp(Q_Slerp(q12,q23,u),Q_Slerp(q23,q34,u),u) - : [for(ui=u) Q_Scubic( q1,q2,q3,q4,ui) ]; - - // Function: Q_exp() // Usage: // q2 = Q_exp(q); diff --git a/tests/test_all.scad b/tests/test_all.scad deleted file mode 100644 index fe6512b..0000000 --- a/tests/test_all.scad +++ /dev/null @@ -1,28 +0,0 @@ -//include -include -include -include -include -include -include -include -include -include -include -include -include -include -//include -include -include -include -include -include -include -include -include -include -include -include - - diff --git a/tests/test_quaternions.scad b/tests/test_quaternions.scad index af19408..b1e5130 100644 --- a/tests/test_quaternions.scad +++ b/tests/test_quaternions.scad @@ -1,80 +1,6 @@ include <../std.scad> include <../strings.scad> -function plane_fit(points) = - assert( is_matrix(points,undef,3) , "Improper point list." ) - len(points)< 2 ? [] : - len(points)==3 ? plane3pt(points[0],points[1],points[2]) : - let( - A = [for(pi=points) concat(pi,-1) ], - qr = qr_factor(A), - R = select(qr[1],0,3), -x=[for(ri=R) echo(R=ri)0], -//y=[for(qi=qr[0]) echo(Q=qi)0], - s = back_substitute - ( R, - sum([for(qi=qr[0])[for(j=[0:3]) qi[j] ] ]) - ) -//, s0 = echo(ls=linear_solve(A,[for(p=points) 0])) -,w=s==[]? echo("s == []")0:echo(s=s)0 - ) - s==[] - ? // points are collinear - [] - : // plane through the origin? - approx(norm([s.x, s.y, s.z]), 0) - ? let( - k = max_index([for(i=[0:2]) abs(s[i]) ]), - A = [[1,0,0], for(pi=points) pi ], - b = [1, for(i=[1:len(points)]) 0], - s = linear_solve(A,b) -, y=echo(s2=s) - ) - s==[]? []: - concat(s, 0)/norm(s) - : s/norm([s.x, s.y, s.z]) ; - -function plane_fit(points) = - assert( is_matrix(points,undef,3) , "Improper point list." ) - len(points)< 2 ? [] : - len(points)==3 ? plane3pt(points[0],points[1],points[2]) : - let( - A = [for(pi=points) concat(pi,-1) ], - B = [ for(pi=points) [0,1] ], - sB = transpose(linear_solve(A,B)), - s = sB==[] ? [] - : norm(sB[1])N/2? 10*[1,1,1]: rands(-1,1,3) ]; -pf = plane_fit(pts); -echo(pf); -//pm = concat(sum(pts)/len(pts), -1); -//echo(pmfit=pm*pf); function rec_cmp(a,b,eps=1e-9) = typeof(a)!=typeof(b)? false : @@ -82,18 +8,7 @@ function rec_cmp(a,b,eps=1e-9) = is_list(a)? len(a)==len(b) && all([for (i=idx(a)) rec_cmp(a[i],b[i],eps=eps)]) : a == b; -function standardize(v) = - v==[] - ? [] - : sign(first_nonzero(v))*v; - -function first_nonzero(v) = - v==[] ? 0 - : is_num(v) ? v - : [for(vi=v) if(!is_list(vi) && ! approx(vi,0)) vi - else if(is_list(vi)) first_nonzero(vi), 0 ][0]; - -module assert_std(vc,ve) { assert_approx(standardize(vc),standardize(ve)); } +function Qstandard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q; module verify_f(actual,expected) { if (!rec_cmp(actual,expected)) { @@ -110,7 +25,7 @@ module verify_f(actual,expected) { module test_is_quat() { verify_f(Q_is_quat([0]),false); - verify_f(Q_is_quat([0,0,0,0]),true); + 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); } @@ -189,7 +104,7 @@ test_QuatXYZ(); module test_Q_From_to() { verify_f(Q_Mul(Q_From_to([1,2,3], [4,5,2]),Q_From_to([4,5,2], [1,2,3])), Q_Ident()); - verify_f(Q_to_matrix4(Q_From_to([1,2,3], [4,5,2])), rot(from=[1,2,3],to=[4,5,2])); + 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])); } @@ -285,7 +200,7 @@ test_Q_Mul(); 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]]); } -*test_Q_Cumulative(); +test_Q_Cumulative(); module test_Q_Dot() { @@ -304,18 +219,18 @@ test_Q_Neg(); module test_Q_Conj() { - ang = rands(0,360,3); 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]); - verify_f(Q_Mul(Q_Conj(QuatXYZ(ang)),QuatXYZ(ang)),Q_Ident()); } test_Q_Conj(); module test_Q_Inverse() { - verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]); - verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]); + + verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]/sqrt(2)); + verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]/sqrt(2)); + verify_f(Q_Inverse(QuatXYZ([23,45,67])),Q_Conj(QuatXYZ([23,45,67]))); verify_f(Q_Mul(Q_Inverse(QuatXYZ([23,45,67])),QuatXYZ([23,45,67])),Q_Ident()); } test_Q_Inverse(); @@ -332,7 +247,7 @@ test_Q_Norm(); 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]); + verify_f(Q_Normalize(QuatXYZ([23,45,67])),[-0.0533818345, 0.4143703268, 0.4360652669, 0.7970537592]); } test_Q_Normalize(); @@ -345,47 +260,28 @@ test_Q_Dist(); module test_Q_Slerp() { - u = rands(0,1,1)[0]; - ul = rands(0,1,5); - ul2 = [1,1,1,1,1]-ul; - a1 = rands(0,360,1)[0]; - a2 = rands(0,360,1)[0]; - a3 = rands(0,360,1)[0]; - verify_f(standardize(Q_Slerp(QuatX(a1),QuatY(a2),0.0)), standardize(QuatX(a1))); - verify_f(standardize(Q_Slerp(QuatX(45),QuatY(30),0.5)), - [0.1967063121, 0.1330377423, 0, 0.9713946602]); - verify_f(standardize(Q_Slerp(QuatX(a1),QuatY(a2),1.0)), standardize(QuatY(a2))); - verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),-QuatY(a2),u)), - standardize(Q_Slerp(QuatXYZ([a1,a2,0]), QuatY(a2),u))); - verify_f(standardize(Q_Slerp(QuatX(a1),QuatX(a1),u)), standardize(QuatX(a1))); - verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),QuatXYZ([a2,0,a1]),u)), - standardize(Q_Slerp(QuatXYZ([a2,0,a1]),QuatXYZ([a1,a2,0]),1-u))); - verify_f(standardize(Q_Slerp(QuatXYZ([a1,a2,0]),QuatXYZ([a2,0,a1]),ul)), - standardize(Q_Slerp(QuatXYZ([a2,0,a1]),QuatXYZ([a1,a2,0]),ul2))); + 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)); } test_Q_Slerp(); -module test_Q_to_matrix3() { - rotZ_37 = rot(37); - rotZ_37_3 = [for(i=[0:2]) [for(j=[0:2]) rotZ_37[i][j] ] ]; - angs = [12,-49,40]; - rot4 = rot(angs); - rot3 = [for(i=[0:2]) [for(j=[0:2]) rot4[i][j] ] ]; - verify_f(Q_to_matrix3(QuatZ(37)),rotZ_37_3); - verify_f(Q_to_matrix3(QuatXYZ(angs)),rot3); +module test_Q_Matrix3() { + verify_f(Q_Matrix3(QuatZ(37)),rot(37,planar=true)); + verify_f(Q_Matrix3(QuatZ(-49)),rot(-49,planar=true)); } -test_Q_to_matrix3(); +test_Q_Matrix3(); -module test_Q_to_matrix4() { - verify_f(Q_to_matrix4(QuatZ(37)),rot(37)); - verify_f(Q_to_matrix4(QuatZ(-49)),rot(-49)); - verify_f(Q_to_matrix4(QuatX(37)),rot([37,0,0])); - verify_f(Q_to_matrix4(QuatY(37)),rot([0,37,0])); - verify_f(Q_to_matrix4(QuatXYZ([12,34,56])),rot([12,34,56])); +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])); } -test_Q_to_matrix4(); +test_Q_Matrix4(); module test_Q_Axis() { @@ -425,23 +321,23 @@ module test_Qrot() { test_Qrot(); -module test_Q_from_matrix() { - verify_f(standardize(Q_from_matrix(Q_to_matrix3(Quat([12,34,56],33)))),standardize(Quat([12,34,56],33))); - verify_f(Q_to_matrix3(Q_from_matrix(Q_to_matrix3(QuatXYZ([12,34,56])))), - Q_to_matrix3(QuatXYZ([12,34,56]))); +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_from_matrix(); +test_Q_Rotation(); module test_Q_Rotation_path() { - verify_f(Q_Rotation_path(QuatX(135), 5, QuatY(13.5))[0] , Q_to_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), 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)[3] , xrot(11+(55-11)*3/4)); verify_f(Q_Rotation_path(QuatX(11), 5)[4] , xrot(55)); } @@ -451,54 +347,48 @@ 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_to_matrix4(Q_Nlerp(QuatX(135), QuatY(13.5),0.5))); + verify_f(Q_Rotation_path(QuatX(135), 16, QuatY(13.5))[8] , Q_Matrix4(Q_Nlerp(QuatX(135), QuatY(13.5),0.5))); verify_f(Q_Nlerp(QuatX(45),QuatY(30),1.0),QuatY(30)); } test_Q_Nlerp(); module test_Q_Squad() { - u = rands(0,1,5); - su = [1,1,1,1,1]-u; - verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(32),QuatY(30),0.0),QuatX(45)); + 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), + 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(10),QuatZ(20),QuatX(32),QuatXYZ([210,120,30]),u[0] ), - Q_Squad(QuatXYZ([210,120,30]),QuatX(32),QuatZ(20),QuatY(10),1-u[0] ) ); - verify_f( Q_Squad(QuatY(10),QuatZ(20),QuatX(32),QuatXYZ([210,120,30]),u ), - Q_Squad(QuatXYZ([210,120,30]),QuatX(32),QuatZ(20),QuatY(10),su ) ); + verify_f(Q_Squad(QuatY(0),QuatY(0),QuatX(120),QuatX(120),0.3), + Q_Slerp(QuatY(0),QuatX(120),0.3)); } test_Q_Squad(); module test_Q_exp() { - q=QuatXYZ(rands(0,360,3)); 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,4])), [1,2,3,4]); - verify_f(Q_exp(Q_ln(q)), q); - verify_f(Q_exp(q+Q_Inverse(q)),Q_Mul(Q_exp(q),Q_exp(Q_Inverse(q)))); + verify_f(Q_exp(Q_ln([1,2,3,0])), [1,2,3,0]); + verify_f(Q_exp(Q_ln(QuatXYZ([31,27,34]))), QuatXYZ([31,27,34])); + let(q=QuatXYZ([12,23,34])) + verify_f(Q_exp(q+Q_Inverse(q)),Q_Mul(Q_exp(q),Q_exp(Q_Inverse(q)))); } test_Q_exp(); module test_Q_ln() { - q = QuatXYZ(rands(0,360,3)); 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(q)), q); - verify_f(Q_ln(q)+Q_ln(Q_Conj(q)), [0,0,0,0]); + verify_f(Q_ln(Q_exp(QuatXYZ([13,37,43]))), QuatXYZ([13,37,43])); + verify_f(Q_ln(QuatXYZ([12,23,34]))+Q_ln(Q_Inverse(QuatXYZ([12,23,34]))), [0,0,0,0]); } test_Q_ln(); module test_Q_pow() { - q = QuatXYZ(rands(0,360,3)); + 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)); From 99b18d631195c8b7a58ca7b62b487fd197ebe0a3 Mon Sep 17 00:00:00 2001 From: RonaldoCMP Date: Wed, 26 Aug 2020 14:14:26 +0100 Subject: [PATCH 5/5] Correction of C_times arg validation --- math.scad | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/math.scad b/math.scad index 888d048..e5c4625 100644 --- a/math.scad +++ b/math.scad @@ -1175,7 +1175,7 @@ function deriv3(data, h=1, closed=false) = // Description: // Multiplies two complex numbers represented by 2D vectors. function C_times(z1,z2) = - assert( is_vector(z1+z2,2), "Complex numbers should be represented by 2D vectors." ) + assert( is_matrix([z1,z2],2,2), "Complex numbers should be represented by 2D vectors" ) [ z1.x*z2.x - z1.y*z2.y, z1.x*z2.y + z1.y*z2.x ]; // Function: C_div()