mirror of
https://github.com/BelfrySCAD/BOSL2.git
synced 2024-12-29 16:29:40 +00:00
Merge pull request #575 from revarbat/revarbat_dev
Rename quaternion functions to not have uppercase names.
This commit is contained in:
Revar Desmera
GitHub
commit
e0625491ee
4 changed files with 519 additions and 537 deletions
14
paths.scad
14
paths.scad
|
|
@ -1109,11 +1109,11 @@ module spiral_sweep(poly, h, r, twist=360, higbee, center, r1, r2, d, d1, d2, hi
|
|||
// path = [ [0, 0, 0], [33, 33, 33], [66, 33, 40], [100, 0, 0], [150,0,0] ];
|
||||
// path_extrude(path) circle(r=10, $fn=6);
|
||||
module path_extrude(path, convexity=10, clipsize=100) {
|
||||
function polyquats(path, q=Q_Ident(), v=[0,0,1], i=0) = let(
|
||||
function polyquats(path, q=q_ident(), v=[0,0,1], i=0) = let(
|
||||
v2 = path[i+1] - path[i],
|
||||
ang = vector_angle(v,v2),
|
||||
axis = ang>0.001? unit(cross(v,v2)) : [0,0,1],
|
||||
newq = Q_Mul(Quat(axis, ang), q),
|
||||
newq = q_mul(quat(axis, ang), q),
|
||||
dist = norm(v2)
|
||||
) i < (len(path)-2)?
|
||||
concat([[dist, newq, ang]], polyquats(path, newq, v2, i+1)) :
|
||||
|
|
@ -1129,7 +1129,7 @@ module path_extrude(path, convexity=10, clipsize=100) {
|
|||
q = pquats[i][1];
|
||||
difference() {
|
||||
translate(pt1) {
|
||||
Qrot(q) {
|
||||
q_rot(q) {
|
||||
down(clipsize/2/2) {
|
||||
if ((dist+clipsize/2) > 0) {
|
||||
linear_extrude(height=dist+clipsize/2, convexity=convexity) {
|
||||
|
|
@ -1140,12 +1140,12 @@ module path_extrude(path, convexity=10, clipsize=100) {
|
|||
}
|
||||
}
|
||||
translate(pt1) {
|
||||
hq = (i > 0)? Q_Slerp(q, pquats[i-1][1], 0.5) : q;
|
||||
Qrot(hq) down(clipsize/2+epsilon) cube(clipsize, center=true);
|
||||
hq = (i > 0)? q_slerp(q, pquats[i-1][1], 0.5) : q;
|
||||
q_rot(hq) down(clipsize/2+epsilon) cube(clipsize, center=true);
|
||||
}
|
||||
translate(pt2) {
|
||||
hq = (i < ptcount-2)? Q_Slerp(q, pquats[i+1][1], 0.5) : q;
|
||||
Qrot(hq) up(clipsize/2+epsilon) cube(clipsize, center=true);
|
||||
hq = (i < ptcount-2)? q_slerp(q, pquats[i+1][1], 0.5) : q;
|
||||
q_rot(hq) up(clipsize/2+epsilon) cube(clipsize, center=true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
483
quaternions.scad
483
quaternions.scad
|
|
@ -16,190 +16,190 @@
|
|||
|
||||
|
||||
// Internal
|
||||
function _Quat(a,s,w) = [a[0]*s, a[1]*s, a[2]*s, w];
|
||||
function _quat(a,s,w) = [a[0]*s, a[1]*s, a[2]*s, w];
|
||||
|
||||
function _Qvec(q) = [q.x,q.y,q.z];
|
||||
function _qvec(q) = [q.x,q.y,q.z];
|
||||
|
||||
function _Qreal(q) = q[3];
|
||||
function _qreal(q) = q[3];
|
||||
|
||||
function _Qset(v,r) = concat( v, r );
|
||||
function _qset(v,r) = concat( v, r );
|
||||
|
||||
// normalizes without checking
|
||||
function _Qnorm(q) = q/norm(q);
|
||||
function _qnorm(q) = q/norm(q);
|
||||
|
||||
|
||||
// Function: Q_is_quat()
|
||||
// Function: is_quaternion()
|
||||
// Usage:
|
||||
// if(Q_is_quat(q)) a=0;
|
||||
// if(is_quaternion(q)) a=0;
|
||||
// Description: Return true if q is a valid non-zero quaternion.
|
||||
// Arguments:
|
||||
// q = object to check.
|
||||
function Q_is_quat(q) = is_vector(q,4) && ! approx(norm(q),0) ;
|
||||
function is_quaternion(q) = is_vector(q,4) && ! approx(norm(q),0) ;
|
||||
|
||||
|
||||
// Function: Quat()
|
||||
// Function: quat()
|
||||
// Usage:
|
||||
// Quat(ax, ang);
|
||||
// quat(ax, ang);
|
||||
// Description: Create a normalized Quaternion from axis and angle of rotation.
|
||||
// Arguments:
|
||||
// ax = Vector of axis of rotation.
|
||||
// ang = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
||||
function Quat(ax=[0,0,1], ang=0) =
|
||||
function quat(ax=[0,0,1], ang=0) =
|
||||
assert( is_vector(ax,3) && is_finite(ang), "Invalid input")
|
||||
let( n = norm(ax) )
|
||||
approx(n,0)
|
||||
? _Quat([0,0,0], sin(ang/2), cos(ang/2))
|
||||
: _Quat(ax/n, sin(ang/2), cos(ang/2));
|
||||
? _quat([0,0,0], sin(ang/2), cos(ang/2))
|
||||
: _quat(ax/n, sin(ang/2), cos(ang/2));
|
||||
|
||||
|
||||
// Function: QuatX()
|
||||
// Function: quat_x()
|
||||
// Usage:
|
||||
// QuatX(a);
|
||||
// quat_x(a);
|
||||
// Description: Create a normalized Quaternion for rotating around the X axis [1,0,0].
|
||||
// Arguments:
|
||||
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
||||
function QuatX(a=0) =
|
||||
function quat_x(a=0) =
|
||||
assert( is_finite(a), "Invalid angle" )
|
||||
Quat([1,0,0],a);
|
||||
quat([1,0,0],a);
|
||||
|
||||
|
||||
// Function: QuatY()
|
||||
// Function: quat_y()
|
||||
// Usage:
|
||||
// QuatY(a);
|
||||
// quat_y(a);
|
||||
// Description: Create a normalized Quaternion for rotating around the Y axis [0,1,0].
|
||||
// Arguments:
|
||||
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
||||
function QuatY(a=0) =
|
||||
function quat_y(a=0) =
|
||||
assert( is_finite(a), "Invalid angle" )
|
||||
Quat([0,1,0],a);
|
||||
quat([0,1,0],a);
|
||||
|
||||
|
||||
// Function: QuatZ()
|
||||
// Function: quat_z()
|
||||
// Usage:
|
||||
// QuatZ(a);
|
||||
// quat_z(a);
|
||||
// Description: Create a normalized Quaternion for rotating around the Z axis [0,0,1].
|
||||
// Arguments:
|
||||
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
||||
function QuatZ(a=0) =
|
||||
function quat_z(a=0) =
|
||||
assert( is_finite(a), "Invalid angle" )
|
||||
Quat([0,0,1],a);
|
||||
quat([0,0,1],a);
|
||||
|
||||
|
||||
// Function: QuatXYZ()
|
||||
// Function: quat_xyz()
|
||||
// Usage:
|
||||
// QuatXYZ([X,Y,Z])
|
||||
// quat_xyz([X,Y,Z])
|
||||
// Description:
|
||||
// Creates a normalized quaternion from standard [X,Y,Z] rotation angles in degrees.
|
||||
// Arguments:
|
||||
// a = The triplet of rotation angles, [X,Y,Z]
|
||||
function QuatXYZ(a=[0,0,0]) =
|
||||
function quat_xyz(a=[0,0,0]) =
|
||||
assert( is_vector(a,3), "Invalid angles")
|
||||
let(
|
||||
qx = QuatX(a[0]),
|
||||
qy = QuatY(a[1]),
|
||||
qz = QuatZ(a[2])
|
||||
qx = quat_x(a[0]),
|
||||
qy = quat_y(a[1]),
|
||||
qz = quat_z(a[2])
|
||||
)
|
||||
Q_Mul(qz, Q_Mul(qy, qx));
|
||||
q_mul(qz, q_mul(qy, qx));
|
||||
|
||||
|
||||
// Function: Q_From_to()
|
||||
// Function: q_from_to()
|
||||
// Usage:
|
||||
// q = Q_From_to(v1, v2);
|
||||
// q = q_from_to(v1, v2);
|
||||
// Description:
|
||||
// Returns the normalized quaternion that rotates the non zero 3D vector v1
|
||||
// to the non zero 3D vector v2.
|
||||
function Q_From_to(v1, v2) =
|
||||
function q_from_to(v1, v2) =
|
||||
assert( is_vector(v1,3) && is_vector(v2,3)
|
||||
&& ! approx(norm(v1),0) && ! approx(norm(v2),0)
|
||||
, "Invalid vector(s)")
|
||||
let( ax = cross(v1,v2),
|
||||
n = norm(ax) )
|
||||
approx(n, 0)
|
||||
? v1*v2>0 ? Q_Ident() : Quat([ v1.y, -v1.x, 0], 180)
|
||||
: Quat(ax, atan2( n , v1*v2 ));
|
||||
? v1*v2>0 ? q_ident() : quat([ v1.y, -v1.x, 0], 180)
|
||||
: quat(ax, atan2( n , v1*v2 ));
|
||||
|
||||
|
||||
// Function: Q_Ident()
|
||||
// Function: q_ident()
|
||||
// Description: Returns the "Identity" zero-rotation Quaternion.
|
||||
function Q_Ident() = [0, 0, 0, 1];
|
||||
function q_ident() = [0, 0, 0, 1];
|
||||
|
||||
|
||||
// Function: Q_Add_S()
|
||||
// Function: q_add_s()
|
||||
// Usage:
|
||||
// Q_Add_S(q, s)
|
||||
// q_add_s(q, s)
|
||||
// Description:
|
||||
// Adds a scalar value `s` to the W part of a quaternion `q`.
|
||||
// The returned quaternion is usually not normalized.
|
||||
function Q_Add_S(q, s) =
|
||||
function q_add_s(q, s) =
|
||||
assert( is_finite(s), "Invalid scalar" )
|
||||
q+[0,0,0,s];
|
||||
|
||||
|
||||
// Function: Q_Sub_S()
|
||||
// Function: q_sub_s()
|
||||
// Usage:
|
||||
// Q_Sub_S(q, s)
|
||||
// q_sub_s(q, s)
|
||||
// Description:
|
||||
// Subtracts a scalar value `s` from the W part of a quaternion `q`.
|
||||
// The returned quaternion is usually not normalized.
|
||||
function Q_Sub_S(q, s) =
|
||||
function q_sub_s(q, s) =
|
||||
assert( is_finite(s), "Invalid scalar" )
|
||||
q-[0,0,0,s];
|
||||
|
||||
|
||||
// Function: Q_Mul_S()
|
||||
// Function: q_mul_s()
|
||||
// Usage:
|
||||
// Q_Mul_S(q, s)
|
||||
// q_mul_s(q, s)
|
||||
// Description:
|
||||
// Multiplies each part of a quaternion `q` by a scalar value `s`.
|
||||
// The returned quaternion is usually not normalized.
|
||||
function Q_Mul_S(q, s) =
|
||||
function q_mul_s(q, s) =
|
||||
assert( is_finite(s), "Invalid scalar" )
|
||||
q*s;
|
||||
|
||||
|
||||
// Function: Q_Div_S()
|
||||
// Function: q_div_s()
|
||||
// Usage:
|
||||
// Q_Div_S(q, s)
|
||||
// q_div_s(q, s)
|
||||
// Description:
|
||||
// Divides each part of a quaternion `q` by a scalar value `s`.
|
||||
// The returned quaternion is usually not normalized.
|
||||
function Q_Div_S(q, s) =
|
||||
function q_div_s(q, s) =
|
||||
assert( is_finite(s) && ! approx(s,0) , "Invalid scalar" )
|
||||
q/s;
|
||||
|
||||
|
||||
// Function: Q_Add()
|
||||
// Function: q_add()
|
||||
// Usage:
|
||||
// Q_Add(a, b)
|
||||
// q_add(a, b)
|
||||
// Description:
|
||||
// Adds each part of two quaternions together.
|
||||
// The returned quaternion is usually not normalized.
|
||||
function Q_Add(a, b) =
|
||||
assert( Q_is_quat(a) && Q_is_quat(a), "Invalid quaternion(s)")
|
||||
function q_add(a, b) =
|
||||
assert( is_quaternion(a) && is_quaternion(a), "Invalid quaternion(s)")
|
||||
assert( ! approx(norm(a+b),0), "Quaternions cannot be opposed" )
|
||||
a+b;
|
||||
|
||||
|
||||
// Function: Q_Sub()
|
||||
// Function: q_sub()
|
||||
// Usage:
|
||||
// Q_Sub(a, b)
|
||||
// q_sub(a, b)
|
||||
// Description:
|
||||
// Subtracts each part of quaternion `b` from quaternion `a`.
|
||||
// The returned quaternion is usually not normalized.
|
||||
function Q_Sub(a, b) =
|
||||
assert( Q_is_quat(a) && Q_is_quat(a), "Invalid quaternion(s)")
|
||||
function q_sub(a, b) =
|
||||
assert( is_quaternion(a) && is_quaternion(a), "Invalid quaternion(s)")
|
||||
assert( ! approx(a,b), "Quaternions cannot be equal" )
|
||||
a-b;
|
||||
|
||||
|
||||
// Function: Q_Mul()
|
||||
// Function: q_mul()
|
||||
// Usage:
|
||||
// Q_Mul(a, b)
|
||||
// q_mul(a, b)
|
||||
// Description:
|
||||
// Multiplies quaternion `a` by quaternion `b`.
|
||||
// The returned quaternion is normalized if both `a` and `b` are normalized
|
||||
function Q_Mul(a, b) =
|
||||
assert( Q_is_quat(a) && Q_is_quat(b), "Invalid quaternion(s)")
|
||||
function q_mul(a, b) =
|
||||
assert( is_quaternion(a) && is_quaternion(b), "Invalid quaternion(s)")
|
||||
[
|
||||
a[3]*b.x + a.x*b[3] + a.y*b.z - a.z*b.y,
|
||||
a[3]*b.y - a.x*b.z + a.y*b[3] + a.z*b.x,
|
||||
|
|
@ -208,94 +208,94 @@ function Q_Mul(a, b) =
|
|||
];
|
||||
|
||||
|
||||
// Function: Q_Cumulative()
|
||||
// Function: q_cumulative()
|
||||
// Usage:
|
||||
// Q_Cumulative(v);
|
||||
// q_cumulative(v);
|
||||
// Description:
|
||||
// Given a list of Quaternions, cumulatively multiplies them, returning a list
|
||||
// of each cumulative Quaternion product. It starts with the first quaternion
|
||||
// given in the list, and applies successive quaternion rotations in list order.
|
||||
// The quaternion in the returned list are normalized if each quaternion in v
|
||||
// is normalized.
|
||||
function Q_Cumulative(v, _i=0, _acc=[]) =
|
||||
function q_cumulative(v, _i=0, _acc=[]) =
|
||||
_i==len(v) ? _acc :
|
||||
Q_Cumulative(
|
||||
q_cumulative(
|
||||
v, _i+1,
|
||||
concat(
|
||||
_acc,
|
||||
[_i==0 ? v[_i] : Q_Mul(v[_i], last(_acc))]
|
||||
[_i==0 ? v[_i] : q_mul(v[_i], last(_acc))]
|
||||
)
|
||||
);
|
||||
|
||||
|
||||
// Function: Q_Dot()
|
||||
// Function: q_dot()
|
||||
// Usage:
|
||||
// Q_Dot(a, b)
|
||||
// q_dot(a, b)
|
||||
// Description: Calculates the dot product between quaternions `a` and `b`.
|
||||
function Q_Dot(a, b) =
|
||||
assert( Q_is_quat(a) && Q_is_quat(b), "Invalid quaternion(s)" )
|
||||
function q_dot(a, b) =
|
||||
assert( is_quaternion(a) && is_quaternion(b), "Invalid quaternion(s)" )
|
||||
a*b;
|
||||
|
||||
// Function: Q_Neg()
|
||||
// Function: q_neg()
|
||||
// Usage:
|
||||
// Q_Neg(q)
|
||||
// q_neg(q)
|
||||
// Description: Returns the negative of quaternion `q`.
|
||||
function Q_Neg(q) =
|
||||
assert( Q_is_quat(q), "Invalid quaternion" )
|
||||
function q_neg(q) =
|
||||
assert( is_quaternion(q), "Invalid quaternion" )
|
||||
-q;
|
||||
|
||||
|
||||
// Function: Q_Conj()
|
||||
// Function: q_conj()
|
||||
// Usage:
|
||||
// Q_Conj(q)
|
||||
// q_conj(q)
|
||||
// Description: Returns the conjugate of quaternion `q`.
|
||||
function Q_Conj(q) =
|
||||
assert( Q_is_quat(q), "Invalid quaternion" )
|
||||
function q_conj(q) =
|
||||
assert( is_quaternion(q), "Invalid quaternion" )
|
||||
[-q.x, -q.y, -q.z, q[3]];
|
||||
|
||||
|
||||
// Function: Q_Inverse()
|
||||
// Function: q_inverse()
|
||||
// Usage:
|
||||
// qc = Q_Inverse(q)
|
||||
// qc = q_inverse(q)
|
||||
// Description: Returns the multiplication inverse of quaternion `q` that is normalized only if `q` is normalized.
|
||||
function Q_Inverse(q) =
|
||||
assert( Q_is_quat(q), "Invalid quaternion" )
|
||||
let(q = _Qnorm(q) )
|
||||
function q_inverse(q) =
|
||||
assert( is_quaternion(q), "Invalid quaternion" )
|
||||
let(q = _qnorm(q) )
|
||||
[-q.x, -q.y, -q.z, q[3]];
|
||||
|
||||
|
||||
// Function: Q_Norm()
|
||||
// Function: q_norm()
|
||||
// Usage:
|
||||
// Q_Norm(q)
|
||||
// q_norm(q)
|
||||
// Description:
|
||||
// Returns the `norm()` "length" of quaternion `q`.
|
||||
// Normalized quaternions have unitary norm.
|
||||
function Q_Norm(q) =
|
||||
assert( Q_is_quat(q), "Invalid quaternion" )
|
||||
function q_norm(q) =
|
||||
assert( is_quaternion(q), "Invalid quaternion" )
|
||||
norm(q);
|
||||
|
||||
|
||||
// Function: Q_Normalize()
|
||||
// Function: q_normalize()
|
||||
// Usage:
|
||||
// Q_Normalize(q)
|
||||
// q_normalize(q)
|
||||
// Description: Normalizes quaternion `q`, so that norm([W,X,Y,Z]) == 1.
|
||||
function Q_Normalize(q) =
|
||||
assert( Q_is_quat(q) , "Invalid quaternion" )
|
||||
function q_normalize(q) =
|
||||
assert( is_quaternion(q) , "Invalid quaternion" )
|
||||
q/norm(q);
|
||||
|
||||
|
||||
// Function: Q_Dist()
|
||||
// Function: q_dist()
|
||||
// Usage:
|
||||
// Q_Dist(q1, q2)
|
||||
// q_dist(q1, q2)
|
||||
// Description: Returns the "distance" between two quaternions.
|
||||
function Q_Dist(q1, q2) =
|
||||
assert( Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" )
|
||||
function q_dist(q1, q2) =
|
||||
assert( is_quaternion(q1) && is_quaternion(q2), "Invalid quaternion(s)" )
|
||||
norm(q2-q1);
|
||||
|
||||
|
||||
// Function: Q_Slerp()
|
||||
// Function: q_slerp()
|
||||
// Usage:
|
||||
// Q_Slerp(q1, q2, u);
|
||||
// q_slerp(q1, q2, u);
|
||||
// Description:
|
||||
// Returns a quaternion that is a spherical interpolation between two quaternions.
|
||||
// Arguments:
|
||||
|
|
@ -303,45 +303,45 @@ function Q_Dist(q1, q2) =
|
|||
// q2 = The second quaternion. (u=1)
|
||||
// u = The proportional value, from 0 to 1, of what part of the interpolation to return.
|
||||
// Example(3D): Giving `u` as a Scalar
|
||||
// a = QuatY(-135);
|
||||
// b = QuatXYZ([0,-30,30]);
|
||||
// a = quat_y(-135);
|
||||
// b = quat_xyz([0,-30,30]);
|
||||
// for (u=[0:0.1:1])
|
||||
// Qrot(Q_Slerp(a, b, u))
|
||||
// q_rot(q_slerp(a, b, u))
|
||||
// right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
// Example(3D): Giving `u` as a Range
|
||||
// a = QuatZ(-135);
|
||||
// b = QuatXYZ([90,0,-45]);
|
||||
// for (q = Q_Slerp(a, b, [0:0.1:1]))
|
||||
// Qrot(q) right(80) cube([10,10,1]);
|
||||
// a = quat_z(-135);
|
||||
// b = quat_xyz([90,0,-45]);
|
||||
// for (q = q_slerp(a, b, [0:0.1:1]))
|
||||
// q_rot(q) right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
function Q_Slerp(q1, q2, u, _dot) =
|
||||
function q_slerp(q1, q2, u, _dot) =
|
||||
is_undef(_dot)
|
||||
? assert(is_finite(u) || is_range(u) || is_vector(u), "Invalid interpolation coefficient(s)")
|
||||
assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" )
|
||||
assert(is_quaternion(q1) && is_quaternion(q2), "Invalid quaternion(s)" )
|
||||
let(
|
||||
_dot = q1*q2,
|
||||
q1 = q1/norm(q1),
|
||||
q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2),
|
||||
dot = abs(_dot)
|
||||
)
|
||||
! is_finite(u) ? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] :
|
||||
Q_Slerp(q1, q2, u, dot)
|
||||
! is_finite(u) ? [for (uu=u) q_slerp(q1, q2, uu, dot)] :
|
||||
q_slerp(q1, q2, u, dot)
|
||||
: _dot>0.9995
|
||||
? _Qnorm(q1 + u*(q2-q1))
|
||||
? _qnorm(q1 + u*(q2-q1))
|
||||
: let( theta = u*acos(_dot),
|
||||
q3 = _Qnorm(q2 - _dot*q1)
|
||||
q3 = _qnorm(q2 - _dot*q1)
|
||||
)
|
||||
_Qnorm(q1*cos(theta) + q3*sin(theta));
|
||||
_qnorm(q1*cos(theta) + q3*sin(theta));
|
||||
|
||||
|
||||
// Function: Q_Matrix3()
|
||||
// Function: q_matrix3()
|
||||
// Usage:
|
||||
// Q_Matrix3(q);
|
||||
// q_matrix3(q);
|
||||
// Description:
|
||||
// Returns the 3x3 rotation matrix for the given normalized quaternion q.
|
||||
function Q_Matrix3(q) =
|
||||
let( q = Q_Normalize(q) )
|
||||
function q_matrix3(q) =
|
||||
let( q = q_normalize(q) )
|
||||
[
|
||||
[1-2*q[1]*q[1]-2*q[2]*q[2], 2*q[0]*q[1]-2*q[2]*q[3], 2*q[0]*q[2]+2*q[1]*q[3]],
|
||||
[ 2*q[0]*q[1]+2*q[2]*q[3], 1-2*q[0]*q[0]-2*q[2]*q[2], 2*q[1]*q[2]-2*q[0]*q[3]],
|
||||
|
|
@ -349,13 +349,13 @@ function Q_Matrix3(q) =
|
|||
];
|
||||
|
||||
|
||||
// Function: Q_Matrix4()
|
||||
// Function: q_matrix4()
|
||||
// Usage:
|
||||
// Q_Matrix4(q);
|
||||
// q_matrix4(q);
|
||||
// Description:
|
||||
// Returns the 4x4 rotation matrix for the given normalized quaternion q.
|
||||
function Q_Matrix4(q) =
|
||||
let( q = Q_Normalize(q) )
|
||||
function q_matrix4(q) =
|
||||
let( q = q_normalize(q) )
|
||||
[
|
||||
[1-2*q[1]*q[1]-2*q[2]*q[2], 2*q[0]*q[1]-2*q[2]*q[3], 2*q[0]*q[2]+2*q[1]*q[3], 0],
|
||||
[ 2*q[0]*q[1]+2*q[2]*q[3], 1-2*q[0]*q[0]-2*q[2]*q[2], 2*q[1]*q[2]-2*q[0]*q[3], 0],
|
||||
|
|
@ -364,115 +364,115 @@ function Q_Matrix4(q) =
|
|||
];
|
||||
|
||||
|
||||
// Function: Q_Axis()
|
||||
// Function: q_axis()
|
||||
// Usage:
|
||||
// Q_Axis(q)
|
||||
// q_axis(q)
|
||||
// Description:
|
||||
// Returns the axis of rotation of a normalized quaternion `q`.
|
||||
// The input doesn't need to be normalized.
|
||||
function Q_Axis(q) =
|
||||
assert( Q_is_quat(q) , "Invalid quaternion" )
|
||||
let( d = norm(_Qvec(q)) )
|
||||
approx(d,0)? [0,0,1] : _Qvec(q)/d;
|
||||
function q_axis(q) =
|
||||
assert( is_quaternion(q) , "Invalid quaternion" )
|
||||
let( d = norm(_qvec(q)) )
|
||||
approx(d,0)? [0,0,1] : _qvec(q)/d;
|
||||
|
||||
// Function: Q_Angle()
|
||||
// Function: q_angle()
|
||||
// Usage:
|
||||
// a = Q_Angle(q)
|
||||
// a12 = Q_Angle(q1,q2);
|
||||
// a = q_angle(q)
|
||||
// a12 = q_angle(q1,q2);
|
||||
// Description:
|
||||
// If only q1 is given, returns the angle of rotation (in degrees) of that quaternion.
|
||||
// If both q1 and q2 are given, returns the angle (in degrees) between them.
|
||||
// The input quaternions don't need to be normalized.
|
||||
function Q_Angle(q1,q2) =
|
||||
assert(Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2)), "Invalid quaternion(s)" )
|
||||
let( n1 = is_undef(q2)? norm(_Qvec(q1)): norm(q1) )
|
||||
function q_angle(q1,q2) =
|
||||
assert(is_quaternion(q1) && (is_undef(q2) || is_quaternion(q2)), "Invalid quaternion(s)" )
|
||||
let( n1 = is_undef(q2)? norm(_qvec(q1)): norm(q1) )
|
||||
is_undef(q2)
|
||||
? 2 * atan2(n1,_Qreal(q1))
|
||||
? 2 * atan2(n1,_qreal(q1))
|
||||
: let( q1 = q1/norm(q1),
|
||||
q2 = q2/norm(q2) )
|
||||
4 * atan2(norm(q1 - q2), norm(q1 + q2));
|
||||
|
||||
// Function&Module: Qrot()
|
||||
// Function&Module: q_rot()
|
||||
// Usage: As Module
|
||||
// Qrot(q) ...
|
||||
// q_rot(q) ...
|
||||
// Usage: As Function
|
||||
// pts = Qrot(q,p);
|
||||
// pts = q_rot(q,p);
|
||||
// Description:
|
||||
// When called as a module, rotates all children by the rotation stored in quaternion `q`.
|
||||
// When called as a function with a `p` argument, rotates the point or list of points in `p` by the rotation stored in quaternion `q`.
|
||||
// When called as a function without a `p` argument, returns the affine3d rotation matrix for the rotation stored in quaternion `q`.
|
||||
// Example(FlatSpin,VPD=225,VPT=[71,-26,16]):
|
||||
// module shape() translate([80,0,0]) cube([10,10,1]);
|
||||
// q = QuatXYZ([90,-15,-45]);
|
||||
// Qrot(q) shape();
|
||||
// q = quat_xyz([90,-15,-45]);
|
||||
// q_rot(q) shape();
|
||||
// #shape();
|
||||
// Example(NORENDER):
|
||||
// q = QuatXYZ([45,35,10]);
|
||||
// mat4x4 = Qrot(q);
|
||||
// q = quat_xyz([45,35,10]);
|
||||
// mat4x4 = q_rot(q);
|
||||
// Example(NORENDER):
|
||||
// q = QuatXYZ([45,35,10]);
|
||||
// pt = Qrot(q, p=[4,5,6]);
|
||||
// q = quat_xyz([45,35,10]);
|
||||
// pt = q_rot(q, p=[4,5,6]);
|
||||
// Example(NORENDER):
|
||||
// q = QuatXYZ([45,35,10]);
|
||||
// pts = Qrot(q, p=[[2,3,4], [4,5,6], [9,2,3]]);
|
||||
module Qrot(q) {
|
||||
multmatrix(Q_Matrix4(q)) {
|
||||
// q = quat_xyz([45,35,10]);
|
||||
// pts = q_rot(q, p=[[2,3,4], [4,5,6], [9,2,3]]);
|
||||
module q_rot(q) {
|
||||
multmatrix(q_matrix4(q)) {
|
||||
children();
|
||||
}
|
||||
}
|
||||
|
||||
function Qrot(q,p) =
|
||||
is_undef(p)? Q_Matrix4(q) :
|
||||
is_vector(p)? Qrot(q,[p])[0] :
|
||||
apply(Q_Matrix4(q), p);
|
||||
function q_rot(q,p) =
|
||||
is_undef(p)? q_matrix4(q) :
|
||||
is_vector(p)? q_rot(q,[p])[0] :
|
||||
apply(q_matrix4(q), p);
|
||||
|
||||
|
||||
// Module: Qrot_copies()
|
||||
// Module: q_rot_copies()
|
||||
// Usage:
|
||||
// Qrot_copies(quats) ...
|
||||
// q_rot_copies(quats) ...
|
||||
// Description:
|
||||
// For each quaternion given in the list `quats`, rotates to that orientation and creates a copy
|
||||
// of all children. This is equivalent to `for (q=quats) Qrot(q) ...`.
|
||||
// of all children. This is equivalent to `for (q=quats) q_rot(q) ...`.
|
||||
// Arguments:
|
||||
// quats = A list containing all quaternions to rotate to and create copies of all children for.
|
||||
// Example:
|
||||
// a = QuatZ(-135);
|
||||
// b = QuatXYZ([0,-30,30]);
|
||||
// Qrot_copies(Q_Slerp(a, b, [0:0.1:1]))
|
||||
// a = quat_z(-135);
|
||||
// b = quat_xyz([0,-30,30]);
|
||||
// q_rot_copies(q_slerp(a, b, [0:0.1:1]))
|
||||
// right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
module Qrot_copies(quats) for (q=quats) Qrot(q) children();
|
||||
module q_rot_copies(quats) for (q=quats) q_rot(q) children();
|
||||
|
||||
|
||||
// Function: Q_Rotation()
|
||||
// Function: q_rotation()
|
||||
// Usage:
|
||||
// Q_Rotation(R)
|
||||
// q_rotation(R)
|
||||
// Description:
|
||||
// Returns a normalized quaternion corresponding to the rotation matrix R.
|
||||
// R may be a 3x3 rotation matrix or a homogeneous 4x4 rotation matrix.
|
||||
// The last row and last column of R are ignored for 4x4 matrices.
|
||||
// It doesn't check whether R is in fact a rotation matrix.
|
||||
// If R is not a rotation, the returned quaternion is an unpredictable quaternion .
|
||||
function Q_Rotation(R) =
|
||||
function q_rotation(R) =
|
||||
assert( is_matrix(R,3,3) || is_matrix(R,4,4) ,
|
||||
"Matrix is neither 3x3 nor 4x4")
|
||||
let( tr = R[0][0]+R[1][1]+R[2][2] ) // R trace
|
||||
tr>0
|
||||
? let( r = 1+tr )
|
||||
_Qnorm( _Qset([ R[1][2]-R[2][1], R[2][0]-R[0][2], R[0][1]-R[1][0] ], -r ) )
|
||||
_qnorm( _qset([ R[1][2]-R[2][1], R[2][0]-R[0][2], R[0][1]-R[1][0] ], -r ) )
|
||||
: let( i = max_index([ R[0][0], R[1][1], R[2][2] ]),
|
||||
r = 1 + 2*R[i][i] -R[0][0] -R[1][1] -R[2][2] )
|
||||
i==0 ? _Qnorm( _Qset( [ 4*r, (R[1][0]+R[0][1]), (R[0][2]+R[2][0]) ], (R[2][1]-R[1][2])) ):
|
||||
i==1 ? _Qnorm( _Qset( [ (R[1][0]+R[0][1]), 4*r, (R[2][1]+R[1][2]) ], (R[0][2]-R[2][0])) ):
|
||||
_Qnorm( _Qset( [ (R[2][0]+R[0][2]), (R[1][2]+R[2][1]), 4*r ], (R[1][0]-R[0][1])) ) ;
|
||||
i==0 ? _qnorm( _qset( [ 4*r, (R[1][0]+R[0][1]), (R[0][2]+R[2][0]) ], (R[2][1]-R[1][2])) ):
|
||||
i==1 ? _qnorm( _qset( [ (R[1][0]+R[0][1]), 4*r, (R[2][1]+R[1][2]) ], (R[0][2]-R[2][0])) ):
|
||||
_qnorm( _qset( [ (R[2][0]+R[0][2]), (R[1][2]+R[2][1]), 4*r ], (R[1][0]-R[0][1])) ) ;
|
||||
|
||||
|
||||
// Function&Module: Q_Rotation_path()
|
||||
// Function&Module: q_rotation_path()
|
||||
// Usage: As a function
|
||||
// path = Q_Rotation_path(q1, n, q2);
|
||||
// path = Q_Rotation_path(q1, n);
|
||||
// path = q_rotation_path(q1, n, q2);
|
||||
// path = q_rotation_path(q1, n);
|
||||
// Usage: As a module
|
||||
// Q_Rotation_path(q1, n, q2) ...
|
||||
// q_rotation_path(q1, n, q2) ...
|
||||
// Description:
|
||||
// If q2 is undef and it is called as a function, the path, with length n+1 (n>=1), will be the
|
||||
// cumulative multiplications of the matrix rotation of q1 by itself.
|
||||
|
|
@ -488,50 +488,50 @@ function Q_Rotation(R) =
|
|||
// q2 = The quaternion of the last rotation.
|
||||
// n = An integer defining the path length ( path length = n+1).
|
||||
// Example(3D): as a function
|
||||
// a = QuatY(-135);
|
||||
// b = QuatXYZ([0,-30,30]);
|
||||
// for (M=Q_Rotation_path(a, 10, b))
|
||||
// a = quat_y(-135);
|
||||
// b = quat_xyz([0,-30,30]);
|
||||
// for (M=q_rotation_path(a, 10, b))
|
||||
// multmatrix(M)
|
||||
// right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
// Example(3D): as a module
|
||||
// a = QuatY(-135);
|
||||
// b = QuatXYZ([0,-30,30]);
|
||||
// Q_Rotation_path(a, 10, b)
|
||||
// a = quat_y(-135);
|
||||
// b = quat_xyz([0,-30,30]);
|
||||
// q_rotation_path(a, 10, b)
|
||||
// right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
// Example(3D): as a function
|
||||
// a = QuatY(5);
|
||||
// for (M=Q_Rotation_path(a, 10))
|
||||
// a = quat_y(5);
|
||||
// for (M=q_rotation_path(a, 10))
|
||||
// multmatrix(M)
|
||||
// right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
// Example(3D): as a module
|
||||
// a = QuatY(5);
|
||||
// Q_Rotation_path(a, 10)
|
||||
// a = quat_y(5);
|
||||
// q_rotation_path(a, 10)
|
||||
// right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
function Q_Rotation_path(q1, n=1, q2) =
|
||||
assert( Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2) ), "Invalid quaternion(s)" )
|
||||
function q_rotation_path(q1, n=1, q2) =
|
||||
assert( is_quaternion(q1) && (is_undef(q2) || is_quaternion(q2) ), "Invalid quaternion(s)" )
|
||||
assert( is_finite(n) && n>=1 && n==floor(n), "Invalid integer" )
|
||||
assert( is_undef(q2) || ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" )
|
||||
is_undef(q2)
|
||||
? [for( i=0, dR=Q_Matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R]
|
||||
: let( q2 = Q_Normalize( q1*q2<0 ? -q2: q2 ),
|
||||
dq = Q_pow( Q_Mul( q2, Q_Inverse(q1) ), 1/n ),
|
||||
dR = Q_Matrix4(dq) )
|
||||
[for( i=0, R=Q_Matrix4(q1); i<=n; i=i+1, R=dR*R ) R];
|
||||
? [for( i=0, dR=q_matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R]
|
||||
: let( q2 = q_normalize( q1*q2<0 ? -q2: q2 ),
|
||||
dq = q_pow( q_mul( q2, q_inverse(q1) ), 1/n ),
|
||||
dR = q_matrix4(dq) )
|
||||
[for( i=0, R=q_matrix4(q1); i<=n; i=i+1, R=dR*R ) R];
|
||||
|
||||
module Q_Rotation_path(q1, n=1, q2) {
|
||||
for(Mi=Q_Rotation_path(q1, n, q2))
|
||||
module q_rotation_path(q1, n=1, q2) {
|
||||
for(Mi=q_rotation_path(q1, n, q2))
|
||||
multmatrix(Mi)
|
||||
children();
|
||||
}
|
||||
|
||||
|
||||
// Function: Q_Nlerp()
|
||||
// Function: q_nlerp()
|
||||
// Usage:
|
||||
// q = Q_Nlerp(q1, q2, u);
|
||||
// q = q_nlerp(q1, q2, u);
|
||||
// Description:
|
||||
// Returns a quaternion that is a normalized linear interpolation between two quaternions
|
||||
// when u is a number.
|
||||
|
|
@ -543,33 +543,33 @@ module Q_Rotation_path(q1, n=1, q2) {
|
|||
// q2 = The second quaternion. (u=1)
|
||||
// u = A value (or a list of values), between 0 and 1, of the proportion(s) of each quaternion in the interpolation.
|
||||
// Example(3D): Giving `u` as a Scalar
|
||||
// a = QuatY(-135);
|
||||
// b = QuatXYZ([0,-30,30]);
|
||||
// a = quat_y(-135);
|
||||
// b = quat_xyz([0,-30,30]);
|
||||
// for (u=[0:0.1:1])
|
||||
// Qrot(Q_Nlerp(a, b, u))
|
||||
// q_rot(q_nlerp(a, b, u))
|
||||
// right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
// Example(3D): Giving `u` as a Range
|
||||
// a = QuatZ(-135);
|
||||
// b = QuatXYZ([90,0,-45]);
|
||||
// for (q = Q_Nlerp(a, b, [0:0.1:1]))
|
||||
// Qrot(q) right(80) cube([10,10,1]);
|
||||
// a = quat_z(-135);
|
||||
// b = quat_xyz([90,0,-45]);
|
||||
// for (q = q_nlerp(a, b, [0:0.1:1]))
|
||||
// q_rot(q) right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
function Q_Nlerp(q1,q2,u) =
|
||||
function q_nlerp(q1,q2,u) =
|
||||
assert(is_finite(u) || is_range(u) || is_vector(u) ,
|
||||
"Invalid interpolation coefficient(s)" )
|
||||
assert(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" )
|
||||
assert(is_quaternion(q1) && is_quaternion(q2), "Invalid quaternion(s)" )
|
||||
assert( ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" )
|
||||
let( q1 = Q_Normalize(q1),
|
||||
q2 = Q_Normalize(q2) )
|
||||
let( q1 = q_normalize(q1),
|
||||
q2 = q_normalize(q2) )
|
||||
is_num(u)
|
||||
? _Qnorm((1-u)*q1 + u*q2 )
|
||||
: [for (ui=u) _Qnorm((1-ui)*q1 + ui*q2 ) ];
|
||||
? _qnorm((1-u)*q1 + u*q2 )
|
||||
: [for (ui=u) _qnorm((1-ui)*q1 + ui*q2 ) ];
|
||||
|
||||
|
||||
// Function: Q_Squad()
|
||||
// Function: q_squad()
|
||||
// Usage:
|
||||
// qn = Q_Squad(q1,q2,q3,q4,u);
|
||||
// qn = q_squad(q1,q2,q3,q4,u);
|
||||
// Description:
|
||||
// Returns a quaternion that is a cubic spherical interpolation of the quaternions
|
||||
// q1 and q4 taking the other two quaternions, q2 and q3, as parameter of a cubic
|
||||
|
|
@ -586,71 +586,72 @@ function Q_Nlerp(q1,q2,u) =
|
|||
// q4 = The end quaternion. (u=1)
|
||||
// u = A value (or a list of values), of the proportion(s) of each quaternion in the cubic interpolation.
|
||||
// Example(3D): Giving `u` as a Scalar
|
||||
// a = QuatY(-135);
|
||||
// b = QuatXYZ([-50,-50,120]);
|
||||
// c = QuatXYZ([-50,-40,30]);
|
||||
// d = QuatY(-45);
|
||||
// a = quat_y(-135);
|
||||
// b = quat_xyz([-50,-50,120]);
|
||||
// c = quat_xyz([-50,-40,30]);
|
||||
// d = quat_y(-45);
|
||||
// color("red"){
|
||||
// Qrot(b) right(80) cube([10,10,1]);
|
||||
// Qrot(c) right(80) cube([10,10,1]);
|
||||
// q_rot(b) right(80) cube([10,10,1]);
|
||||
// q_rot(c) right(80) cube([10,10,1]);
|
||||
// }
|
||||
// for (u=[0:0.05:1])
|
||||
// Qrot(Q_Squad(a, b, c, d, u))
|
||||
// q_rot(q_squad(a, b, c, d, u))
|
||||
// right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
// Example(3D): Giving `u` as a Range
|
||||
// a = QuatY(-135);
|
||||
// b = QuatXYZ([-50,-50,120]);
|
||||
// c = QuatXYZ([-50,-40,30]);
|
||||
// d = QuatY(-45);
|
||||
// for (q = Q_Squad(a, b, c, d, [0:0.05:1]))
|
||||
// Qrot(q) right(80) cube([10,10,1]);
|
||||
// a = quat_y(-135);
|
||||
// b = quat_xyz([-50,-50,120]);
|
||||
// c = quat_xyz([-50,-40,30]);
|
||||
// d = quat_y(-45);
|
||||
// for (q = q_squad(a, b, c, d, [0:0.05:1]))
|
||||
// q_rot(q) right(80) cube([10,10,1]);
|
||||
// #sphere(r=80);
|
||||
function Q_Squad(q1,q2,q3,q4,u) =
|
||||
function q_squad(q1,q2,q3,q4,u) =
|
||||
assert(is_finite(u) || is_range(u) || is_vector(u) ,
|
||||
"Invalid interpolation coefficient(s)" )
|
||||
is_num(u)
|
||||
? Q_Slerp( Q_Slerp(q1,q4,u), Q_Slerp(q2,q3,u), 2*u*(1-u))
|
||||
: [for(ui=u) Q_Slerp( Q_Slerp(q1,q4,ui), Q_Slerp(q2,q3,ui), 2*ui*(1-ui) ) ];
|
||||
? q_slerp( q_slerp(q1,q4,u), q_slerp(q2,q3,u), 2*u*(1-u))
|
||||
: [for(ui=u) q_slerp( q_slerp(q1,q4,ui), q_slerp(q2,q3,ui), 2*ui*(1-ui) ) ];
|
||||
|
||||
|
||||
// Function: Q_exp()
|
||||
// Function: q_exp()
|
||||
// Usage:
|
||||
// q2 = Q_exp(q);
|
||||
// q2 = q_exp(q);
|
||||
// Description:
|
||||
// Returns the quaternion that is the exponential of the quaternion q in base e
|
||||
// The returned quaternion is usually not normalized.
|
||||
function Q_exp(q) =
|
||||
function q_exp(q) =
|
||||
assert( is_vector(q,4), "Input is not a valid quaternion")
|
||||
let( nv = norm(_Qvec(q)) ) // q may be equal to zero here!
|
||||
exp(_Qreal(q))*Quat(_Qvec(q),2*nv);
|
||||
let( nv = norm(_qvec(q)) ) // q may be equal to zero here!
|
||||
exp(_qreal(q))*quat(_qvec(q),2*nv);
|
||||
|
||||
|
||||
// Function: Q_ln()
|
||||
// Function: q_ln()
|
||||
// Usage:
|
||||
// q2 = Q_ln(q);
|
||||
// q2 = q_ln(q);
|
||||
// Description:
|
||||
// Returns the quaternion that is the natural logarithm of the quaternion q.
|
||||
// The returned quaternion is usually not normalized and may be zero.
|
||||
function Q_ln(q) =
|
||||
assert(Q_is_quat(q), "Input is not a valid quaternion")
|
||||
let( nq = norm(q),
|
||||
nv = norm(_Qvec(q)) )
|
||||
approx(nv,0) ? _Qset([0,0,0] , ln(nq) ) :
|
||||
_Qset(_Qvec(q)*atan2(nv,_Qreal(q))/nv, ln(nq));
|
||||
function q_ln(q) =
|
||||
assert(is_quaternion(q), "Input is not a valid quaternion")
|
||||
let(
|
||||
nq = norm(q),
|
||||
nv = norm(_qvec(q))
|
||||
)
|
||||
approx(nv,0) ? _qset([0,0,0] , ln(nq) ) :
|
||||
_qset(_qvec(q)*atan2(nv,_qreal(q))/nv, ln(nq));
|
||||
|
||||
|
||||
// Function: Q_pow()
|
||||
// Function: q_pow()
|
||||
// Usage:
|
||||
// q2 = Q_pow(q, r);
|
||||
// q2 = q_pow(q, r);
|
||||
// Description:
|
||||
// Returns the quaternion that is the power of the quaternion q to the real exponent r.
|
||||
// The returned quaternion is normalized if `q` is normalized.
|
||||
function Q_pow(q,r=1) =
|
||||
assert( Q_is_quat(q) && is_finite(r),
|
||||
"Invalid inputs")
|
||||
let( theta = 2*atan2(norm(_Qvec(q)),_Qreal(q)) )
|
||||
Quat(_Qvec(q), r*theta); // Q_exp(r*Q_ln(q));
|
||||
function q_pow(q,r=1) =
|
||||
assert( is_quaternion(q) && is_finite(r), "Invalid inputs")
|
||||
let( theta = 2*atan2(norm(_qvec(q)),_qreal(q)) )
|
||||
quat(_qvec(q), r*theta); // q_exp(r*q_ln(q));
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -307,15 +307,15 @@ module stroke(
|
|||
multmatrix(mat) polygon(endcap_shape2);
|
||||
}
|
||||
} else {
|
||||
quatsums = Q_Cumulative([
|
||||
quatsums = q_cumulative([
|
||||
for (i = idx(path2,e=-2)) let(
|
||||
vec1 = i==0? UP : unit(path2[i]-path2[i-1], UP),
|
||||
vec2 = unit(path2[i+1]-path2[i], UP),
|
||||
axis = vector_axis(vec1,vec2),
|
||||
ang = vector_angle(vec1,vec2)
|
||||
) Quat(axis,ang)
|
||||
) quat(axis,ang)
|
||||
]);
|
||||
rotmats = [for (q=quatsums) Q_Matrix4(q)];
|
||||
rotmats = [for (q=quatsums) q_matrix4(q)];
|
||||
sides = [
|
||||
for (i = idx(path2,e=-2))
|
||||
quantup(segs(max(widths[i],widths[i+1])/2),4)
|
||||
|
|
|
|||
|
|
@ -1,403 +1,384 @@
|
|||
include <../std.scad>
|
||||
include <../strings.scad>
|
||||
|
||||
|
||||
function rec_cmp(a,b,eps=1e-9) =
|
||||
typeof(a)!=typeof(b)? false :
|
||||
is_num(a)? approx(a,b,eps=eps) :
|
||||
is_list(a)? len(a)==len(b) && all([for (i=idx(a)) rec_cmp(a[i],b[i],eps=eps)]) :
|
||||
a == b;
|
||||
|
||||
function Qstandard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q;
|
||||
function _q_standard(q) = sign([for(qi=q) if( ! approx(qi,0)) qi,0 ][0])*q;
|
||||
|
||||
module verify_f(actual,expected) {
|
||||
if (!rec_cmp(actual,expected)) {
|
||||
echo(str("Expected: ",fmt_float(expected,10)));
|
||||
echo(str(" : ",expected));
|
||||
echo(str("Actual : ",fmt_float(actual,10)));
|
||||
echo(str(" : ",actual));
|
||||
echo(str("Delta : ",fmt_float(expected-actual,10)));
|
||||
echo(str(" : ",expected-actual));
|
||||
assert(approx(expected,actual));
|
||||
}
|
||||
|
||||
module test_is_quaternion() {
|
||||
assert_approx(is_quaternion([0]),false);
|
||||
assert_approx(is_quaternion([0,0,0,0]),false);
|
||||
assert_approx(is_quaternion([1,0,2,0]),true);
|
||||
assert_approx(is_quaternion([1,0,2,0,0]),false);
|
||||
}
|
||||
test_is_quaternion();
|
||||
|
||||
|
||||
module test_Q_is_quat() {
|
||||
verify_f(Q_is_quat([0]),false);
|
||||
verify_f(Q_is_quat([0,0,0,0]),false);
|
||||
verify_f(Q_is_quat([1,0,2,0]),true);
|
||||
verify_f(Q_is_quat([1,0,2,0,0]),false);
|
||||
module test_quat() {
|
||||
assert_approx(quat(UP,0),[0,0,0,1]);
|
||||
assert_approx(quat(FWD,0),[0,0,0,1]);
|
||||
assert_approx(quat(LEFT,0),[0,0,0,1]);
|
||||
assert_approx(quat(UP,45),[0,0,0.3826834324,0.9238795325]);
|
||||
assert_approx(quat(LEFT,45),[-0.3826834324, 0, 0, 0.9238795325]);
|
||||
assert_approx(quat(BACK,45),[0,0.3826834323,0,0.9238795325]);
|
||||
assert_approx(quat(FWD+RIGHT,30),[0.1830127019, -0.1830127019, 0, 0.9659258263]);
|
||||
}
|
||||
test_Q_is_quat();
|
||||
test_quat();
|
||||
|
||||
|
||||
module test_Quat() {
|
||||
verify_f(Quat(UP,0),[0,0,0,1]);
|
||||
verify_f(Quat(FWD,0),[0,0,0,1]);
|
||||
verify_f(Quat(LEFT,0),[0,0,0,1]);
|
||||
verify_f(Quat(UP,45),[0,0,0.3826834324,0.9238795325]);
|
||||
verify_f(Quat(LEFT,45),[-0.3826834324, 0, 0, 0.9238795325]);
|
||||
verify_f(Quat(BACK,45),[0,0.3826834323,0,0.9238795325]);
|
||||
verify_f(Quat(FWD+RIGHT,30),[0.1830127019, -0.1830127019, 0, 0.9659258263]);
|
||||
module test_quat_x() {
|
||||
assert_approx(quat_x(0),[0,0,0,1]);
|
||||
assert_approx(quat_x(35),[0.3007057995,0,0,0.9537169507]);
|
||||
assert_approx(quat_x(45),[0.3826834324,0,0,0.9238795325]);
|
||||
}
|
||||
test_Quat();
|
||||
test_quat_x();
|
||||
|
||||
|
||||
module test_QuatX() {
|
||||
verify_f(QuatX(0),[0,0,0,1]);
|
||||
verify_f(QuatX(35),[0.3007057995,0,0,0.9537169507]);
|
||||
verify_f(QuatX(45),[0.3826834324,0,0,0.9238795325]);
|
||||
module test_quat_y() {
|
||||
assert_approx(quat_y(0),[0,0,0,1]);
|
||||
assert_approx(quat_y(35),[0,0.3007057995,0,0.9537169507]);
|
||||
assert_approx(quat_y(45),[0,0.3826834323,0,0.9238795325]);
|
||||
}
|
||||
test_QuatX();
|
||||
test_quat_y();
|
||||
|
||||
|
||||
module test_QuatY() {
|
||||
verify_f(QuatY(0),[0,0,0,1]);
|
||||
verify_f(QuatY(35),[0,0.3007057995,0,0.9537169507]);
|
||||
verify_f(QuatY(45),[0,0.3826834323,0,0.9238795325]);
|
||||
module test_quat_z() {
|
||||
assert_approx(quat_z(0),[0,0,0,1]);
|
||||
assert_approx(quat_z(36),[0,0,0.3090169944,0.9510565163]);
|
||||
assert_approx(quat_z(45),[0,0,0.3826834324,0.9238795325]);
|
||||
}
|
||||
test_QuatY();
|
||||
test_quat_z();
|
||||
|
||||
|
||||
module test_QuatZ() {
|
||||
verify_f(QuatZ(0),[0,0,0,1]);
|
||||
verify_f(QuatZ(36),[0,0,0.3090169944,0.9510565163]);
|
||||
verify_f(QuatZ(45),[0,0,0.3826834324,0.9238795325]);
|
||||
module test_quat_xyz() {
|
||||
assert_approx(quat_xyz([0,0,0]), [0,0,0,1]);
|
||||
assert_approx(quat_xyz([30,0,0]), [0.2588190451, 0, 0, 0.9659258263]);
|
||||
assert_approx(quat_xyz([90,0,0]), [0.7071067812, 0, 0, 0.7071067812]);
|
||||
assert_approx(quat_xyz([-270,0,0]), [-0.7071067812, 0, 0, -0.7071067812]);
|
||||
assert_approx(quat_xyz([180,0,0]), [1,0,0,0]);
|
||||
assert_approx(quat_xyz([270,0,0]), [0.7071067812, 0, 0, -0.7071067812]);
|
||||
assert_approx(quat_xyz([-90,0,0]), [-0.7071067812, 0, 0, 0.7071067812]);
|
||||
assert_approx(quat_xyz([360,0,0]), [0,0,0,-1]);
|
||||
|
||||
assert_approx(quat_xyz([0,0,0]), [0,0,0,1]);
|
||||
assert_approx(quat_xyz([0,30,0]), [0, 0.2588190451, 0, 0.9659258263]);
|
||||
assert_approx(quat_xyz([0,90,0]), [0, 0.7071067812, 0, 0.7071067812]);
|
||||
assert_approx(quat_xyz([0,-270,0]), [0, -0.7071067812, 0, -0.7071067812]);
|
||||
assert_approx(quat_xyz([0,180,0]), [0,1,0,0]);
|
||||
assert_approx(quat_xyz([0,270,0]), [0, 0.7071067812, 0, -0.7071067812]);
|
||||
assert_approx(quat_xyz([0,-90,0]), [0, -0.7071067812, 0, 0.7071067812]);
|
||||
assert_approx(quat_xyz([0,360,0]), [0,0,0,-1]);
|
||||
|
||||
assert_approx(quat_xyz([0,0,0]), [0,0,0,1]);
|
||||
assert_approx(quat_xyz([0,0,30]), [0, 0, 0.2588190451, 0.9659258263]);
|
||||
assert_approx(quat_xyz([0,0,90]), [0, 0, 0.7071067812, 0.7071067812]);
|
||||
assert_approx(quat_xyz([0,0,-270]), [0, 0, -0.7071067812, -0.7071067812]);
|
||||
assert_approx(quat_xyz([0,0,180]), [0,0,1,0]);
|
||||
assert_approx(quat_xyz([0,0,270]), [0, 0, 0.7071067812, -0.7071067812]);
|
||||
assert_approx(quat_xyz([0,0,-90]), [0, 0, -0.7071067812, 0.7071067812]);
|
||||
assert_approx(quat_xyz([0,0,360]), [0,0,0,-1]);
|
||||
|
||||
assert_approx(quat_xyz([30,30,30]), [0.1767766953, 0.3061862178, 0.1767766953, 0.9185586535]);
|
||||
assert_approx(quat_xyz([12,34,56]), [-0.04824789229, 0.3036636044, 0.4195145429, 0.8540890495]);
|
||||
}
|
||||
test_QuatZ();
|
||||
test_quat_xyz();
|
||||
|
||||
|
||||
module test_QuatXYZ() {
|
||||
verify_f(QuatXYZ([0,0,0]), [0,0,0,1]);
|
||||
verify_f(QuatXYZ([30,0,0]), [0.2588190451, 0, 0, 0.9659258263]);
|
||||
verify_f(QuatXYZ([90,0,0]), [0.7071067812, 0, 0, 0.7071067812]);
|
||||
verify_f(QuatXYZ([-270,0,0]), [-0.7071067812, 0, 0, -0.7071067812]);
|
||||
verify_f(QuatXYZ([180,0,0]), [1,0,0,0]);
|
||||
verify_f(QuatXYZ([270,0,0]), [0.7071067812, 0, 0, -0.7071067812]);
|
||||
verify_f(QuatXYZ([-90,0,0]), [-0.7071067812, 0, 0, 0.7071067812]);
|
||||
verify_f(QuatXYZ([360,0,0]), [0,0,0,-1]);
|
||||
|
||||
verify_f(QuatXYZ([0,0,0]), [0,0,0,1]);
|
||||
verify_f(QuatXYZ([0,30,0]), [0, 0.2588190451, 0, 0.9659258263]);
|
||||
verify_f(QuatXYZ([0,90,0]), [0, 0.7071067812, 0, 0.7071067812]);
|
||||
verify_f(QuatXYZ([0,-270,0]), [0, -0.7071067812, 0, -0.7071067812]);
|
||||
verify_f(QuatXYZ([0,180,0]), [0,1,0,0]);
|
||||
verify_f(QuatXYZ([0,270,0]), [0, 0.7071067812, 0, -0.7071067812]);
|
||||
verify_f(QuatXYZ([0,-90,0]), [0, -0.7071067812, 0, 0.7071067812]);
|
||||
verify_f(QuatXYZ([0,360,0]), [0,0,0,-1]);
|
||||
|
||||
verify_f(QuatXYZ([0,0,0]), [0,0,0,1]);
|
||||
verify_f(QuatXYZ([0,0,30]), [0, 0, 0.2588190451, 0.9659258263]);
|
||||
verify_f(QuatXYZ([0,0,90]), [0, 0, 0.7071067812, 0.7071067812]);
|
||||
verify_f(QuatXYZ([0,0,-270]), [0, 0, -0.7071067812, -0.7071067812]);
|
||||
verify_f(QuatXYZ([0,0,180]), [0,0,1,0]);
|
||||
verify_f(QuatXYZ([0,0,270]), [0, 0, 0.7071067812, -0.7071067812]);
|
||||
verify_f(QuatXYZ([0,0,-90]), [0, 0, -0.7071067812, 0.7071067812]);
|
||||
verify_f(QuatXYZ([0,0,360]), [0,0,0,-1]);
|
||||
|
||||
verify_f(QuatXYZ([30,30,30]), [0.1767766953, 0.3061862178, 0.1767766953, 0.9185586535]);
|
||||
verify_f(QuatXYZ([12,34,56]), [-0.04824789229, 0.3036636044, 0.4195145429, 0.8540890495]);
|
||||
module test_q_from_to() {
|
||||
assert_approx(q_mul(q_from_to([1,2,3], [4,5,2]),q_from_to([4,5,2], [1,2,3])), q_ident());
|
||||
assert_approx(q_matrix4(q_from_to([1,2,3], [4,5,2])), rot(from=[1,2,3],to=[4,5,2]));
|
||||
assert_approx(q_rot(q_from_to([1,2,3], -[1,2,3]),[1,2,3]), -[1,2,3]);
|
||||
assert_approx(unit(q_rot(q_from_to([1,2,3], [4,5,2]),[1,2,3])), unit([4,5,2]));
|
||||
}
|
||||
test_QuatXYZ();
|
||||
test_q_from_to();
|
||||
|
||||
|
||||
module test_Q_From_to() {
|
||||
verify_f(Q_Mul(Q_From_to([1,2,3], [4,5,2]),Q_From_to([4,5,2], [1,2,3])), Q_Ident());
|
||||
verify_f(Q_Matrix4(Q_From_to([1,2,3], [4,5,2])), rot(from=[1,2,3],to=[4,5,2]));
|
||||
verify_f(Qrot(Q_From_to([1,2,3], -[1,2,3]),[1,2,3]), -[1,2,3]);
|
||||
verify_f(unit(Qrot(Q_From_to([1,2,3], [4,5,2]),[1,2,3])), unit([4,5,2]));
|
||||
module test_q_ident() {
|
||||
assert_approx(q_ident(), [0,0,0,1]);
|
||||
}
|
||||
test_Q_From_to();
|
||||
test_q_ident();
|
||||
|
||||
|
||||
module test_Q_Ident() {
|
||||
verify_f(Q_Ident(), [0,0,0,1]);
|
||||
module test_q_add_s() {
|
||||
assert_approx(q_add_s([0,0,0,1],3),[0,0,0,4]);
|
||||
assert_approx(q_add_s([0,0,1,0],3),[0,0,1,3]);
|
||||
assert_approx(q_add_s([0,1,0,0],3),[0,1,0,3]);
|
||||
assert_approx(q_add_s([1,0,0,0],3),[1,0,0,3]);
|
||||
assert_approx(q_add_s(quat(LEFT+FWD,23),1),[-0.1409744184, -0.1409744184, 0, 1.979924705]);
|
||||
}
|
||||
test_Q_Ident();
|
||||
test_q_add_s();
|
||||
|
||||
|
||||
module test_Q_Add_S() {
|
||||
verify_f(Q_Add_S([0,0,0,1],3),[0,0,0,4]);
|
||||
verify_f(Q_Add_S([0,0,1,0],3),[0,0,1,3]);
|
||||
verify_f(Q_Add_S([0,1,0,0],3),[0,1,0,3]);
|
||||
verify_f(Q_Add_S([1,0,0,0],3),[1,0,0,3]);
|
||||
verify_f(Q_Add_S(Quat(LEFT+FWD,23),1),[-0.1409744184, -0.1409744184, 0, 1.979924705]);
|
||||
module test_q_sub_s() {
|
||||
assert_approx(q_sub_s([0,0,0,1],3),[0,0,0,-2]);
|
||||
assert_approx(q_sub_s([0,0,1,0],3),[0,0,1,-3]);
|
||||
assert_approx(q_sub_s([0,1,0,0],3),[0,1,0,-3]);
|
||||
assert_approx(q_sub_s([1,0,0,0],3),[1,0,0,-3]);
|
||||
assert_approx(q_sub_s(quat(LEFT+FWD,23),1),[-0.1409744184, -0.1409744184, 0, -0.02007529538]);
|
||||
}
|
||||
test_Q_Add_S();
|
||||
test_q_sub_s();
|
||||
|
||||
|
||||
module test_Q_Sub_S() {
|
||||
verify_f(Q_Sub_S([0,0,0,1],3),[0,0,0,-2]);
|
||||
verify_f(Q_Sub_S([0,0,1,0],3),[0,0,1,-3]);
|
||||
verify_f(Q_Sub_S([0,1,0,0],3),[0,1,0,-3]);
|
||||
verify_f(Q_Sub_S([1,0,0,0],3),[1,0,0,-3]);
|
||||
verify_f(Q_Sub_S(Quat(LEFT+FWD,23),1),[-0.1409744184, -0.1409744184, 0, -0.02007529538]);
|
||||
module test_q_mul_s() {
|
||||
assert_approx(q_mul_s([0,0,0,1],3),[0,0,0,3]);
|
||||
assert_approx(q_mul_s([0,0,1,0],3),[0,0,3,0]);
|
||||
assert_approx(q_mul_s([0,1,0,0],3),[0,3,0,0]);
|
||||
assert_approx(q_mul_s([1,0,0,0],3),[3,0,0,0]);
|
||||
assert_approx(q_mul_s([1,0,0,1],3),[3,0,0,3]);
|
||||
assert_approx(q_mul_s(quat(LEFT+FWD,23),4),[-0.5638976735, -0.5638976735, 0, 3.919698818]);
|
||||
}
|
||||
test_Q_Sub_S();
|
||||
test_q_mul_s();
|
||||
|
||||
|
||||
module test_Q_Mul_S() {
|
||||
verify_f(Q_Mul_S([0,0,0,1],3),[0,0,0,3]);
|
||||
verify_f(Q_Mul_S([0,0,1,0],3),[0,0,3,0]);
|
||||
verify_f(Q_Mul_S([0,1,0,0],3),[0,3,0,0]);
|
||||
verify_f(Q_Mul_S([1,0,0,0],3),[3,0,0,0]);
|
||||
verify_f(Q_Mul_S([1,0,0,1],3),[3,0,0,3]);
|
||||
verify_f(Q_Mul_S(Quat(LEFT+FWD,23),4),[-0.5638976735, -0.5638976735, 0, 3.919698818]);
|
||||
|
||||
module test_q_div_s() {
|
||||
assert_approx(q_div_s([0,0,0,1],3),[0,0,0,1/3]);
|
||||
assert_approx(q_div_s([0,0,1,0],3),[0,0,1/3,0]);
|
||||
assert_approx(q_div_s([0,1,0,0],3),[0,1/3,0,0]);
|
||||
assert_approx(q_div_s([1,0,0,0],3),[1/3,0,0,0]);
|
||||
assert_approx(q_div_s([1,0,0,1],3),[1/3,0,0,1/3]);
|
||||
assert_approx(q_div_s(quat(LEFT+FWD,23),4),[-0.03524360459, -0.03524360459, 0, 0.2449811762]);
|
||||
}
|
||||
test_Q_Mul_S();
|
||||
test_q_div_s();
|
||||
|
||||
|
||||
|
||||
module test_Q_Div_S() {
|
||||
verify_f(Q_Div_S([0,0,0,1],3),[0,0,0,1/3]);
|
||||
verify_f(Q_Div_S([0,0,1,0],3),[0,0,1/3,0]);
|
||||
verify_f(Q_Div_S([0,1,0,0],3),[0,1/3,0,0]);
|
||||
verify_f(Q_Div_S([1,0,0,0],3),[1/3,0,0,0]);
|
||||
verify_f(Q_Div_S([1,0,0,1],3),[1/3,0,0,1/3]);
|
||||
verify_f(Q_Div_S(Quat(LEFT+FWD,23),4),[-0.03524360459, -0.03524360459, 0, 0.2449811762]);
|
||||
module test_q_add() {
|
||||
assert_approx(q_add([2,3,4,5],[-1,-1,-1,-1]),[1,2,3,4]);
|
||||
assert_approx(q_add([2,3,4,5],[-3,-3,-3,-3]),[-1,0,1,2]);
|
||||
assert_approx(q_add([2,3,4,5],[0,0,0,0]),[2,3,4,5]);
|
||||
assert_approx(q_add([2,3,4,5],[1,1,1,1]),[3,4,5,6]);
|
||||
assert_approx(q_add([2,3,4,5],[1,0,0,0]),[3,3,4,5]);
|
||||
assert_approx(q_add([2,3,4,5],[0,1,0,0]),[2,4,4,5]);
|
||||
assert_approx(q_add([2,3,4,5],[0,0,1,0]),[2,3,5,5]);
|
||||
assert_approx(q_add([2,3,4,5],[0,0,0,1]),[2,3,4,6]);
|
||||
assert_approx(q_add([2,3,4,5],[2,1,2,1]),[4,4,6,6]);
|
||||
assert_approx(q_add([2,3,4,5],[1,2,1,2]),[3,5,5,7]);
|
||||
}
|
||||
test_Q_Div_S();
|
||||
test_q_add();
|
||||
|
||||
|
||||
module test_Q_Add() {
|
||||
verify_f(Q_Add([2,3,4,5],[-1,-1,-1,-1]),[1,2,3,4]);
|
||||
verify_f(Q_Add([2,3,4,5],[-3,-3,-3,-3]),[-1,0,1,2]);
|
||||
verify_f(Q_Add([2,3,4,5],[0,0,0,0]),[2,3,4,5]);
|
||||
verify_f(Q_Add([2,3,4,5],[1,1,1,1]),[3,4,5,6]);
|
||||
verify_f(Q_Add([2,3,4,5],[1,0,0,0]),[3,3,4,5]);
|
||||
verify_f(Q_Add([2,3,4,5],[0,1,0,0]),[2,4,4,5]);
|
||||
verify_f(Q_Add([2,3,4,5],[0,0,1,0]),[2,3,5,5]);
|
||||
verify_f(Q_Add([2,3,4,5],[0,0,0,1]),[2,3,4,6]);
|
||||
verify_f(Q_Add([2,3,4,5],[2,1,2,1]),[4,4,6,6]);
|
||||
verify_f(Q_Add([2,3,4,5],[1,2,1,2]),[3,5,5,7]);
|
||||
module test_q_sub() {
|
||||
assert_approx(q_sub([2,3,4,5],[-1,-1,-1,-1]),[3,4,5,6]);
|
||||
assert_approx(q_sub([2,3,4,5],[-3,-3,-3,-3]),[5,6,7,8]);
|
||||
assert_approx(q_sub([2,3,4,5],[0,0,0,0]),[2,3,4,5]);
|
||||
assert_approx(q_sub([2,3,4,5],[1,1,1,1]),[1,2,3,4]);
|
||||
assert_approx(q_sub([2,3,4,5],[1,0,0,0]),[1,3,4,5]);
|
||||
assert_approx(q_sub([2,3,4,5],[0,1,0,0]),[2,2,4,5]);
|
||||
assert_approx(q_sub([2,3,4,5],[0,0,1,0]),[2,3,3,5]);
|
||||
assert_approx(q_sub([2,3,4,5],[0,0,0,1]),[2,3,4,4]);
|
||||
assert_approx(q_sub([2,3,4,5],[2,1,2,1]),[0,2,2,4]);
|
||||
assert_approx(q_sub([2,3,4,5],[1,2,1,2]),[1,1,3,3]);
|
||||
}
|
||||
test_Q_Add();
|
||||
test_q_sub();
|
||||
|
||||
|
||||
module test_Q_Sub() {
|
||||
verify_f(Q_Sub([2,3,4,5],[-1,-1,-1,-1]),[3,4,5,6]);
|
||||
verify_f(Q_Sub([2,3,4,5],[-3,-3,-3,-3]),[5,6,7,8]);
|
||||
verify_f(Q_Sub([2,3,4,5],[0,0,0,0]),[2,3,4,5]);
|
||||
verify_f(Q_Sub([2,3,4,5],[1,1,1,1]),[1,2,3,4]);
|
||||
verify_f(Q_Sub([2,3,4,5],[1,0,0,0]),[1,3,4,5]);
|
||||
verify_f(Q_Sub([2,3,4,5],[0,1,0,0]),[2,2,4,5]);
|
||||
verify_f(Q_Sub([2,3,4,5],[0,0,1,0]),[2,3,3,5]);
|
||||
verify_f(Q_Sub([2,3,4,5],[0,0,0,1]),[2,3,4,4]);
|
||||
verify_f(Q_Sub([2,3,4,5],[2,1,2,1]),[0,2,2,4]);
|
||||
verify_f(Q_Sub([2,3,4,5],[1,2,1,2]),[1,1,3,3]);
|
||||
module test_q_mul() {
|
||||
assert_approx(q_mul(quat_z(30),quat_x(57)),[0.4608999698, 0.1234977747, 0.2274546059, 0.8488721457]);
|
||||
assert_approx(q_mul(quat_y(30),quat_z(23)),[0.05160021841, 0.2536231763, 0.1925746368, 0.94653458]);
|
||||
}
|
||||
test_Q_Sub();
|
||||
test_q_mul();
|
||||
|
||||
|
||||
module test_Q_Mul() {
|
||||
verify_f(Q_Mul(QuatZ(30),QuatX(57)),[0.4608999698, 0.1234977747, 0.2274546059, 0.8488721457]);
|
||||
verify_f(Q_Mul(QuatY(30),QuatZ(23)),[0.05160021841, 0.2536231763, 0.1925746368, 0.94653458]);
|
||||
module test_q_cumulative() {
|
||||
assert_approx(q_cumulative([quat_z(30),quat_x(57),quat_y(18)]),[[0, 0, 0.2588190451, 0.9659258263], [0.4608999698, -0.1234977747, 0.2274546059, 0.8488721457], [0.4908072659, 0.01081554785, 0.1525536221, 0.8577404293]]);
|
||||
}
|
||||
test_Q_Mul();
|
||||
test_q_cumulative();
|
||||
|
||||
|
||||
module test_Q_Cumulative() {
|
||||
verify_f(Q_Cumulative([QuatZ(30),QuatX(57),QuatY(18)]),[[0, 0, 0.2588190451, 0.9659258263], [0.4608999698, -0.1234977747, 0.2274546059, 0.8488721457], [0.4908072659, 0.01081554785, 0.1525536221, 0.8577404293]]);
|
||||
module test_q_dot() {
|
||||
assert_approx(q_dot(quat_z(30),quat_x(57)),0.8488721457);
|
||||
assert_approx(q_dot(quat_y(30),quat_z(23)),0.94653458);
|
||||
}
|
||||
test_Q_Cumulative();
|
||||
test_q_dot();
|
||||
|
||||
|
||||
module test_Q_Dot() {
|
||||
verify_f(Q_Dot(QuatZ(30),QuatX(57)),0.8488721457);
|
||||
verify_f(Q_Dot(QuatY(30),QuatZ(23)),0.94653458);
|
||||
module test_q_neg() {
|
||||
assert_approx(q_neg([1,0,0,1]),[-1,0,0,-1]);
|
||||
assert_approx(q_neg([0,1,1,0]),[0,-1,-1,0]);
|
||||
assert_approx(q_neg(quat_xyz([23,45,67])),[0.0533818345,-0.4143703268,-0.4360652669,-0.7970537592]);
|
||||
}
|
||||
test_Q_Dot();
|
||||
test_q_neg();
|
||||
|
||||
|
||||
module test_Q_Neg() {
|
||||
verify_f(Q_Neg([1,0,0,1]),[-1,0,0,-1]);
|
||||
verify_f(Q_Neg([0,1,1,0]),[0,-1,-1,0]);
|
||||
verify_f(Q_Neg(QuatXYZ([23,45,67])),[0.0533818345,-0.4143703268,-0.4360652669,-0.7970537592]);
|
||||
module test_q_conj() {
|
||||
assert_approx(q_conj([1,0,0,1]),[-1,0,0,1]);
|
||||
assert_approx(q_conj([0,1,1,0]),[0,-1,-1,0]);
|
||||
assert_approx(q_conj(quat_xyz([23,45,67])),[0.0533818345, -0.4143703268, -0.4360652669, 0.7970537592]);
|
||||
}
|
||||
test_Q_Neg();
|
||||
test_q_conj();
|
||||
|
||||
|
||||
module test_Q_Conj() {
|
||||
verify_f(Q_Conj([1,0,0,1]),[-1,0,0,1]);
|
||||
verify_f(Q_Conj([0,1,1,0]),[0,-1,-1,0]);
|
||||
verify_f(Q_Conj(QuatXYZ([23,45,67])),[0.0533818345, -0.4143703268, -0.4360652669, 0.7970537592]);
|
||||
module test_q_inverse() {
|
||||
|
||||
assert_approx(q_inverse([1,0,0,1]),[-1,0,0,1]/sqrt(2));
|
||||
assert_approx(q_inverse([0,1,1,0]),[0,-1,-1,0]/sqrt(2));
|
||||
assert_approx(q_inverse(quat_xyz([23,45,67])),q_conj(quat_xyz([23,45,67])));
|
||||
assert_approx(q_mul(q_inverse(quat_xyz([23,45,67])),quat_xyz([23,45,67])),q_ident());
|
||||
}
|
||||
test_Q_Conj();
|
||||
test_q_inverse();
|
||||
|
||||
|
||||
module test_Q_Inverse() {
|
||||
|
||||
verify_f(Q_Inverse([1,0,0,1]),[-1,0,0,1]/sqrt(2));
|
||||
verify_f(Q_Inverse([0,1,1,0]),[0,-1,-1,0]/sqrt(2));
|
||||
verify_f(Q_Inverse(QuatXYZ([23,45,67])),Q_Conj(QuatXYZ([23,45,67])));
|
||||
verify_f(Q_Mul(Q_Inverse(QuatXYZ([23,45,67])),QuatXYZ([23,45,67])),Q_Ident());
|
||||
module test_q_Norm() {
|
||||
assert_approx(q_norm([1,0,0,1]),1.414213562);
|
||||
assert_approx(q_norm([0,1,1,0]),1.414213562);
|
||||
assert_approx(q_norm(quat_xyz([23,45,67])),1);
|
||||
}
|
||||
test_Q_Inverse();
|
||||
test_q_Norm();
|
||||
|
||||
|
||||
module test_Q_Norm() {
|
||||
verify_f(Q_Norm([1,0,0,1]),1.414213562);
|
||||
verify_f(Q_Norm([0,1,1,0]),1.414213562);
|
||||
verify_f(Q_Norm(QuatXYZ([23,45,67])),1);
|
||||
module test_q_normalize() {
|
||||
assert_approx(q_normalize([1,0,0,1]),[0.7071067812, 0, 0, 0.7071067812]);
|
||||
assert_approx(q_normalize([0,1,1,0]),[0, 0.7071067812, 0.7071067812, 0]);
|
||||
assert_approx(q_normalize(quat_xyz([23,45,67])),[-0.0533818345, 0.4143703268, 0.4360652669, 0.7970537592]);
|
||||
}
|
||||
test_Q_Norm();
|
||||
test_q_normalize();
|
||||
|
||||
|
||||
module test_Q_Normalize() {
|
||||
verify_f(Q_Normalize([1,0,0,1]),[0.7071067812, 0, 0, 0.7071067812]);
|
||||
verify_f(Q_Normalize([0,1,1,0]),[0, 0.7071067812, 0.7071067812, 0]);
|
||||
verify_f(Q_Normalize(QuatXYZ([23,45,67])),[-0.0533818345, 0.4143703268, 0.4360652669, 0.7970537592]);
|
||||
module test_q_dist() {
|
||||
assert_approx(q_dist(quat_xyz([23,45,67]),quat_xyz([23,45,67])),0);
|
||||
assert_approx(q_dist(quat_xyz([23,45,67]),quat_xyz([12,34,56])),0.1257349854);
|
||||
}
|
||||
test_Q_Normalize();
|
||||
test_q_dist();
|
||||
|
||||
|
||||
module test_Q_Dist() {
|
||||
verify_f(Q_Dist(QuatXYZ([23,45,67]),QuatXYZ([23,45,67])),0);
|
||||
verify_f(Q_Dist(QuatXYZ([23,45,67]),QuatXYZ([12,34,56])),0.1257349854);
|
||||
module test_q_slerp() {
|
||||
assert_approx(q_slerp(quat_x(45),quat_y(30),0.0),quat_x(45));
|
||||
assert_approx(q_slerp(quat_x(45),quat_y(30),0.5),[0.1967063121, 0.1330377423, 0, 0.9713946602]);
|
||||
assert_approx(q_slerp(quat_x(45),quat_y(30),1.0),quat_y(30));
|
||||
}
|
||||
test_Q_Dist();
|
||||
test_q_slerp();
|
||||
|
||||
|
||||
module test_Q_Slerp() {
|
||||
verify_f(Q_Slerp(QuatX(45),QuatY(30),0.0),QuatX(45));
|
||||
verify_f(Q_Slerp(QuatX(45),QuatY(30),0.5),[0.1967063121, 0.1330377423, 0, 0.9713946602]);
|
||||
verify_f(Q_Slerp(QuatX(45),QuatY(30),1.0),QuatY(30));
|
||||
module test_q_matrix3() {
|
||||
assert_approx(q_matrix3(quat_z(37)),rot(37,planar=true));
|
||||
assert_approx(q_matrix3(quat_z(-49)),rot(-49,planar=true));
|
||||
}
|
||||
test_Q_Slerp();
|
||||
test_q_matrix3();
|
||||
|
||||
|
||||
module test_Q_Matrix3() {
|
||||
verify_f(Q_Matrix3(QuatZ(37)),rot(37,planar=true));
|
||||
verify_f(Q_Matrix3(QuatZ(-49)),rot(-49,planar=true));
|
||||
module test_q_matrix4() {
|
||||
assert_approx(q_matrix4(quat_z(37)),rot(37));
|
||||
assert_approx(q_matrix4(quat_z(-49)),rot(-49));
|
||||
assert_approx(q_matrix4(quat_x(37)),rot([37,0,0]));
|
||||
assert_approx(q_matrix4(quat_y(37)),rot([0,37,0]));
|
||||
assert_approx(q_matrix4(quat_xyz([12,34,56])),rot([12,34,56]));
|
||||
}
|
||||
test_Q_Matrix3();
|
||||
test_q_matrix4();
|
||||
|
||||
|
||||
module test_Q_Matrix4() {
|
||||
verify_f(Q_Matrix4(QuatZ(37)),rot(37));
|
||||
verify_f(Q_Matrix4(QuatZ(-49)),rot(-49));
|
||||
verify_f(Q_Matrix4(QuatX(37)),rot([37,0,0]));
|
||||
verify_f(Q_Matrix4(QuatY(37)),rot([0,37,0]));
|
||||
verify_f(Q_Matrix4(QuatXYZ([12,34,56])),rot([12,34,56]));
|
||||
module test_q_axis() {
|
||||
assert_approx(q_axis(quat_x(37)),RIGHT);
|
||||
assert_approx(q_axis(quat_x(-37)),LEFT);
|
||||
assert_approx(q_axis(quat_y(37)),BACK);
|
||||
assert_approx(q_axis(quat_y(-37)),FWD);
|
||||
assert_approx(q_axis(quat_z(37)),UP);
|
||||
assert_approx(q_axis(quat_z(-37)),DOWN);
|
||||
}
|
||||
test_Q_Matrix4();
|
||||
test_q_axis();
|
||||
|
||||
|
||||
module test_Q_Axis() {
|
||||
verify_f(Q_Axis(QuatX(37)),RIGHT);
|
||||
verify_f(Q_Axis(QuatX(-37)),LEFT);
|
||||
verify_f(Q_Axis(QuatY(37)),BACK);
|
||||
verify_f(Q_Axis(QuatY(-37)),FWD);
|
||||
verify_f(Q_Axis(QuatZ(37)),UP);
|
||||
verify_f(Q_Axis(QuatZ(-37)),DOWN);
|
||||
module test_q_angle() {
|
||||
assert_approx(q_angle(quat_x(0)),0);
|
||||
assert_approx(q_angle(quat_y(0)),0);
|
||||
assert_approx(q_angle(quat_z(0)),0);
|
||||
assert_approx(q_angle(quat_x(37)),37);
|
||||
assert_approx(q_angle(quat_x(-37)),37);
|
||||
assert_approx(q_angle(quat_y(37)),37);
|
||||
assert_approx(q_angle(quat_y(-37)),37);
|
||||
assert_approx(q_angle(quat_z(37)),37);
|
||||
assert_approx(q_angle(quat_z(-37)),37);
|
||||
|
||||
assert_approx(q_angle(quat_z(-37),quat_z(-37)), 0);
|
||||
assert_approx(q_angle(quat_z( 37.123),quat_z(-37.123)), 74.246);
|
||||
assert_approx(q_angle(quat_x( 37),quat_y(-37)), 51.86293283);
|
||||
}
|
||||
test_Q_Axis();
|
||||
test_q_angle();
|
||||
|
||||
|
||||
module test_Q_Angle() {
|
||||
verify_f(Q_Angle(QuatX(0)),0);
|
||||
verify_f(Q_Angle(QuatY(0)),0);
|
||||
verify_f(Q_Angle(QuatZ(0)),0);
|
||||
verify_f(Q_Angle(QuatX(37)),37);
|
||||
verify_f(Q_Angle(QuatX(-37)),37);
|
||||
verify_f(Q_Angle(QuatY(37)),37);
|
||||
verify_f(Q_Angle(QuatY(-37)),37);
|
||||
verify_f(Q_Angle(QuatZ(37)),37);
|
||||
verify_f(Q_Angle(QuatZ(-37)),37);
|
||||
|
||||
verify_f(Q_Angle(QuatZ(-37),QuatZ(-37)), 0);
|
||||
verify_f(Q_Angle(QuatZ( 37.123),QuatZ(-37.123)), 74.246);
|
||||
verify_f(Q_Angle(QuatX( 37),QuatY(-37)), 51.86293283);
|
||||
module test_q_rot() {
|
||||
assert_approx(q_rot(quat_xyz([12,34,56])),rot([12,34,56]));
|
||||
assert_approx(q_rot(quat_xyz([12,34,56]),p=[2,3,4]),rot([12,34,56],p=[2,3,4]));
|
||||
assert_approx(q_rot(quat_xyz([12,34,56]),p=[[2,3,4],[4,9,6]]),rot([12,34,56],p=[[2,3,4],[4,9,6]]));
|
||||
}
|
||||
test_Q_Angle();
|
||||
test_q_rot();
|
||||
|
||||
|
||||
module test_Qrot() {
|
||||
verify_f(Qrot(QuatXYZ([12,34,56])),rot([12,34,56]));
|
||||
verify_f(Qrot(QuatXYZ([12,34,56]),p=[2,3,4]),rot([12,34,56],p=[2,3,4]));
|
||||
verify_f(Qrot(QuatXYZ([12,34,56]),p=[[2,3,4],[4,9,6]]),rot([12,34,56],p=[[2,3,4],[4,9,6]]));
|
||||
module test_q_rotation() {
|
||||
assert_approx(_q_standard(q_rotation(q_matrix3(quat([12,34,56],33)))),_q_standard(quat([12,34,56],33)));
|
||||
assert_approx(q_matrix3(q_rotation(q_matrix3(quat_xyz([12,34,56])))),
|
||||
q_matrix3(quat_xyz([12,34,56])));
|
||||
}
|
||||
test_Qrot();
|
||||
test_q_rotation();
|
||||
|
||||
|
||||
module test_Q_Rotation() {
|
||||
verify_f(Qstandard(Q_Rotation(Q_Matrix3(Quat([12,34,56],33)))),Qstandard(Quat([12,34,56],33)));
|
||||
verify_f(Q_Matrix3(Q_Rotation(Q_Matrix3(QuatXYZ([12,34,56])))),
|
||||
Q_Matrix3(QuatXYZ([12,34,56])));
|
||||
}
|
||||
test_Q_Rotation();
|
||||
module test_q_rotation_path() {
|
||||
assert_approx(q_rotation_path(quat_x(135), 5, quat_y(13.5))[0] , q_matrix4(quat_x(135)));
|
||||
assert_approx(q_rotation_path(quat_x(135), 11, quat_y(13.5))[11] , yrot(13.5));
|
||||
assert_approx(q_rotation_path(quat_x(135), 16, quat_y(13.5))[8] , q_rotation_path(quat_x(135), 8, quat_y(13.5))[4]);
|
||||
assert_approx(q_rotation_path(quat_x(135), 16, quat_y(13.5))[7] ,
|
||||
q_rotation_path(quat_y(13.5),16, quat_x(135))[9]);
|
||||
|
||||
|
||||
module test_Q_Rotation_path() {
|
||||
|
||||
verify_f(Q_Rotation_path(QuatX(135), 5, QuatY(13.5))[0] , Q_Matrix4(QuatX(135)));
|
||||
verify_f(Q_Rotation_path(QuatX(135), 11, QuatY(13.5))[11] , yrot(13.5));
|
||||
verify_f(Q_Rotation_path(QuatX(135), 16, QuatY(13.5))[8] , Q_Rotation_path(QuatX(135), 8, QuatY(13.5))[4]);
|
||||
verify_f(Q_Rotation_path(QuatX(135), 16, QuatY(13.5))[7] ,
|
||||
Q_Rotation_path(QuatY(13.5),16, QuatX(135))[9]);
|
||||
|
||||
verify_f(Q_Rotation_path(QuatX(11), 5)[0] , xrot(11));
|
||||
verify_f(Q_Rotation_path(QuatX(11), 5)[4] , xrot(55));
|
||||
assert_approx(q_rotation_path(quat_x(11), 5)[0] , xrot(11));
|
||||
assert_approx(q_rotation_path(quat_x(11), 5)[4] , xrot(55));
|
||||
|
||||
}
|
||||
test_Q_Rotation_path();
|
||||
test_q_rotation_path();
|
||||
|
||||
|
||||
module test_Q_Nlerp() {
|
||||
verify_f(Q_Nlerp(QuatX(45),QuatY(30),0.0),QuatX(45));
|
||||
verify_f(Q_Nlerp(QuatX(45),QuatY(30),0.5),[0.1967063121, 0.1330377423, 0, 0.9713946602]);
|
||||
verify_f(Q_Rotation_path(QuatX(135), 16, QuatY(13.5))[8] , Q_Matrix4(Q_Nlerp(QuatX(135), QuatY(13.5),0.5)));
|
||||
verify_f(Q_Nlerp(QuatX(45),QuatY(30),1.0),QuatY(30));
|
||||
module test_q_nlerp() {
|
||||
assert_approx(q_nlerp(quat_x(45),quat_y(30),0.0),quat_x(45));
|
||||
assert_approx(q_nlerp(quat_x(45),quat_y(30),0.5),[0.1967063121, 0.1330377423, 0, 0.9713946602]);
|
||||
assert_approx(q_rotation_path(quat_x(135), 16, quat_y(13.5))[8] , q_matrix4(q_nlerp(quat_x(135), quat_y(13.5),0.5)));
|
||||
assert_approx(q_nlerp(quat_x(45),quat_y(30),1.0),quat_y(30));
|
||||
}
|
||||
test_Q_Nlerp();
|
||||
test_q_nlerp();
|
||||
|
||||
|
||||
module test_Q_Squad() {
|
||||
verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(90),QuatY(30),0.0),QuatX(45));
|
||||
verify_f(Q_Squad(QuatX(45),QuatZ(30),QuatX(90),QuatY(30),1.0),QuatY(30));
|
||||
verify_f(Q_Squad(QuatX(0),QuatX(30),QuatX(90),QuatX(120),0.5),
|
||||
Q_Slerp(QuatX(0),QuatX(120),0.5));
|
||||
verify_f(Q_Squad(QuatY(0),QuatY(0),QuatX(120),QuatX(120),0.3),
|
||||
Q_Slerp(QuatY(0),QuatX(120),0.3));
|
||||
module test_q_squad() {
|
||||
assert_approx(q_squad(quat_x(45),quat_z(30),quat_x(90),quat_y(30),0.0),quat_x(45));
|
||||
assert_approx(q_squad(quat_x(45),quat_z(30),quat_x(90),quat_y(30),1.0),quat_y(30));
|
||||
assert_approx(q_squad(quat_x(0),quat_x(30),quat_x(90),quat_x(120),0.5),
|
||||
q_slerp(quat_x(0),quat_x(120),0.5));
|
||||
assert_approx(q_squad(quat_y(0),quat_y(0),quat_x(120),quat_x(120),0.3),
|
||||
q_slerp(quat_y(0),quat_x(120),0.3));
|
||||
}
|
||||
test_Q_Squad();
|
||||
test_q_squad();
|
||||
|
||||
|
||||
module test_Q_exp() {
|
||||
verify_f(Q_exp(Q_Ident()), exp(1)*Q_Ident());
|
||||
verify_f(Q_exp([0,0,0,33.7]), exp(33.7)*Q_Ident());
|
||||
verify_f(Q_exp(Q_ln(Q_Ident())), Q_Ident());
|
||||
verify_f(Q_exp(Q_ln([1,2,3,0])), [1,2,3,0]);
|
||||
verify_f(Q_exp(Q_ln(QuatXYZ([31,27,34]))), QuatXYZ([31,27,34]));
|
||||
let(q=QuatXYZ([12,23,34]))
|
||||
verify_f(Q_exp(q+Q_Inverse(q)),Q_Mul(Q_exp(q),Q_exp(Q_Inverse(q))));
|
||||
module test_q_exp() {
|
||||
assert_approx(q_exp(q_ident()), exp(1)*q_ident());
|
||||
assert_approx(q_exp([0,0,0,33.7]), exp(33.7)*q_ident());
|
||||
assert_approx(q_exp(q_ln(q_ident())), q_ident());
|
||||
assert_approx(q_exp(q_ln([1,2,3,0])), [1,2,3,0]);
|
||||
assert_approx(q_exp(q_ln(quat_xyz([31,27,34]))), quat_xyz([31,27,34]));
|
||||
let(q=quat_xyz([12,23,34]))
|
||||
assert_approx(q_exp(q+q_inverse(q)),q_mul(q_exp(q),q_exp(q_inverse(q))));
|
||||
|
||||
}
|
||||
test_Q_exp();
|
||||
test_q_exp();
|
||||
|
||||
|
||||
module test_Q_ln() {
|
||||
verify_f(Q_ln([1,2,3,0]), [24.0535117721, 48.1070235442, 72.1605353164, 1.31952866481]);
|
||||
verify_f(Q_ln(Q_Ident()), [0,0,0,0]);
|
||||
verify_f(Q_ln(5.5*Q_Ident()), [0,0,0,ln(5.5)]);
|
||||
verify_f(Q_ln(Q_exp(QuatXYZ([13,37,43]))), QuatXYZ([13,37,43]));
|
||||
verify_f(Q_ln(QuatXYZ([12,23,34]))+Q_ln(Q_Inverse(QuatXYZ([12,23,34]))), [0,0,0,0]);
|
||||
module test_q_ln() {
|
||||
assert_approx(q_ln([1,2,3,0]), [24.0535117721, 48.1070235442, 72.1605353164, 1.31952866481]);
|
||||
assert_approx(q_ln(q_ident()), [0,0,0,0]);
|
||||
assert_approx(q_ln(5.5*q_ident()), [0,0,0,ln(5.5)]);
|
||||
assert_approx(q_ln(q_exp(quat_xyz([13,37,43]))), quat_xyz([13,37,43]));
|
||||
assert_approx(q_ln(quat_xyz([12,23,34]))+q_ln(q_inverse(quat_xyz([12,23,34]))), [0,0,0,0]);
|
||||
}
|
||||
test_Q_ln();
|
||||
test_q_ln();
|
||||
|
||||
|
||||
module test_Q_pow() {
|
||||
q = Quat([1,2,3],77);
|
||||
verify_f(Q_pow(q,1), q);
|
||||
verify_f(Q_pow(q,0), Q_Ident());
|
||||
verify_f(Q_pow(q,-1), Q_Inverse(q));
|
||||
verify_f(Q_pow(q,2), Q_Mul(q,q));
|
||||
verify_f(Q_pow(q,3), Q_Mul(q,Q_pow(q,2)));
|
||||
verify_f(Q_Mul(Q_pow(q,0.456),Q_pow(q,0.544)), q);
|
||||
verify_f(Q_Mul(Q_pow(q,0.335),Q_Mul(Q_pow(q,.552),Q_pow(q,.113))), q);
|
||||
module test_q_pow() {
|
||||
q = quat([1,2,3],77);
|
||||
assert_approx(q_pow(q,1), q);
|
||||
assert_approx(q_pow(q,0), q_ident());
|
||||
assert_approx(q_pow(q,-1), q_inverse(q));
|
||||
assert_approx(q_pow(q,2), q_mul(q,q));
|
||||
assert_approx(q_pow(q,3), q_mul(q,q_pow(q,2)));
|
||||
assert_approx(q_mul(q_pow(q,0.456),q_pow(q,0.544)), q);
|
||||
assert_approx(q_mul(q_pow(q,0.335),q_mul(q_pow(q,.552),q_pow(q,.113))), q);
|
||||
}
|
||||
test_Q_pow();
|
||||
test_q_pow();
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue