mirror of
https://github.com/BelfrySCAD/BOSL2.git
synced 2025-02-19 01:59:39 +00:00
Changes in Q_Angle computation and formatting
This commit is contained in:
parent
fd0e74ac97
commit
8129176171
1 changed files with 153 additions and 156 deletions
|
@ -47,7 +47,7 @@ function Q_is_quat(q) = is_vector(q,4) && ! approx(norm(q),0) ;
|
||||||
// ax = Vector of axis of rotation.
|
// ax = Vector of axis of rotation.
|
||||||
// ang = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
// 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_num(0*ang), "Invalid input")
|
assert( is_vector(ax,3) && is_finite(ang), "Invalid input")
|
||||||
let( n = norm(ax) )
|
let( n = norm(ax) )
|
||||||
approx(n,0)
|
approx(n,0)
|
||||||
? _Quat([0,0,0], sin(ang/2), cos(ang/2))
|
? _Quat([0,0,0], sin(ang/2), cos(ang/2))
|
||||||
|
@ -61,7 +61,7 @@ function Quat(ax=[0,0,1], ang=0) =
|
||||||
// Arguments:
|
// Arguments:
|
||||||
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
||||||
function QuatX(a=0) =
|
function QuatX(a=0) =
|
||||||
assert( is_num(0*a), "Invalid angle" )
|
assert( is_finite(a), "Invalid angle" )
|
||||||
Quat([1,0,0],a);
|
Quat([1,0,0],a);
|
||||||
|
|
||||||
|
|
||||||
|
@ -72,7 +72,7 @@ function QuatX(a=0) =
|
||||||
// Arguments:
|
// Arguments:
|
||||||
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
||||||
function QuatY(a=0) =
|
function QuatY(a=0) =
|
||||||
assert( is_num(0*a), "Invalid angle" )
|
assert( is_finite(a), "Invalid angle" )
|
||||||
Quat([0,1,0],a);
|
Quat([0,1,0],a);
|
||||||
|
|
||||||
|
|
||||||
|
@ -83,7 +83,7 @@ function QuatY(a=0) =
|
||||||
// Arguments:
|
// Arguments:
|
||||||
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
// a = Number of degrees to rotate around the axis counter-clockwise, when facing the origin.
|
||||||
function QuatZ(a=0) =
|
function QuatZ(a=0) =
|
||||||
assert( is_num(0*a), "Invalid angle" )
|
assert( is_finite(a), "Invalid angle" )
|
||||||
Quat([0,0,1],a);
|
Quat([0,0,1],a);
|
||||||
|
|
||||||
|
|
||||||
|
@ -133,7 +133,7 @@ function Q_Ident() = [0, 0, 0, 1];
|
||||||
// Adds a scalar value `s` to the W part of a quaternion `q`.
|
// Adds a scalar value `s` to the W part of a quaternion `q`.
|
||||||
// The returned quaternion is usually not normalized.
|
// The returned quaternion is usually not normalized.
|
||||||
function Q_Add_S(q, s) =
|
function Q_Add_S(q, s) =
|
||||||
assert( is_num(0*s), "Invalid scalar" )
|
assert( is_finite(s), "Invalid scalar" )
|
||||||
q+[0,0,0,s];
|
q+[0,0,0,s];
|
||||||
|
|
||||||
|
|
||||||
|
@ -144,7 +144,7 @@ function Q_Add_S(q, s) =
|
||||||
// Subtracts a scalar value `s` from the W part of a quaternion `q`.
|
// Subtracts a scalar value `s` from the W part of a quaternion `q`.
|
||||||
// The returned quaternion is usually not normalized.
|
// The returned quaternion is usually not normalized.
|
||||||
function Q_Sub_S(q, s) =
|
function Q_Sub_S(q, s) =
|
||||||
assert( is_num(0*s), "Invalid scalar" )
|
assert( is_finite(s), "Invalid scalar" )
|
||||||
q-[0,0,0,s];
|
q-[0,0,0,s];
|
||||||
|
|
||||||
|
|
||||||
|
@ -155,7 +155,7 @@ function Q_Sub_S(q, s) =
|
||||||
// Multiplies each part of a quaternion `q` by a scalar value `s`.
|
// Multiplies each part of a quaternion `q` by a scalar value `s`.
|
||||||
// The returned quaternion is usually not normalized.
|
// The returned quaternion is usually not normalized.
|
||||||
function Q_Mul_S(q, s) =
|
function Q_Mul_S(q, s) =
|
||||||
assert( is_num(0*s), "Invalid scalar" )
|
assert( is_finite(s), "Invalid scalar" )
|
||||||
q*s;
|
q*s;
|
||||||
|
|
||||||
|
|
||||||
|
@ -166,7 +166,7 @@ function Q_Mul_S(q, s) =
|
||||||
// Divides each part of a quaternion `q` by a scalar value `s`.
|
// Divides each part of a quaternion `q` by a scalar value `s`.
|
||||||
// The returned quaternion is usually not normalized.
|
// The returned quaternion is usually not normalized.
|
||||||
function Q_Div_S(q, s) =
|
function Q_Div_S(q, s) =
|
||||||
assert( is_num(0*s) && ! approx(s,0) , "Invalid scalar" )
|
assert( is_finite(s) && ! approx(s,0) , "Invalid scalar" )
|
||||||
q/s;
|
q/s;
|
||||||
|
|
||||||
|
|
||||||
|
@ -319,7 +319,7 @@ function Q_Dist(q1, q2) =
|
||||||
// #sphere(r=80);
|
// #sphere(r=80);
|
||||||
function Q_Slerp(q1, q2, u, _dot) =
|
function Q_Slerp(q1, q2, u, _dot) =
|
||||||
is_undef(_dot)
|
is_undef(_dot)
|
||||||
? assert(is_num(u) || is_range(u) || is_num(0*u*u), "Invalid interpolation coefficient(s)")
|
? 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(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" )
|
||||||
let(
|
let(
|
||||||
_dot = q1*q2,
|
_dot = q1*q2,
|
||||||
|
@ -327,13 +327,11 @@ function Q_Slerp(q1, q2, u, _dot) =
|
||||||
q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2),
|
q2 = _dot<0 ? -q2/norm(q2) : q2/norm(q2),
|
||||||
dot = abs(_dot)
|
dot = abs(_dot)
|
||||||
)
|
)
|
||||||
! is_num(u)
|
! is_finite(u) ? [for (uu=u) Q_Slerp(q1, q2, uu, dot)] :
|
||||||
? [for (uu=u) Q_Slerp(q1, q2, uu, dot)]
|
Q_Slerp(q1, q2, u, dot)
|
||||||
: Q_Slerp(q1, q2, u, dot)
|
|
||||||
: _dot>0.9995
|
: _dot>0.9995
|
||||||
? _Qnorm(q1 + u*(q2-q1))
|
? _Qnorm(q1 + u*(q2-q1))
|
||||||
: let(
|
: let( theta = u*acos(_dot),
|
||||||
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));
|
||||||
|
@ -392,8 +390,9 @@ function Q_Angle(q1,q2) =
|
||||||
let( n1 = is_undef(q2)? norm(_Qvec(q1)): norm(q1) )
|
let( n1 = is_undef(q2)? norm(_Qvec(q1)): norm(q1) )
|
||||||
is_undef(q2)
|
is_undef(q2)
|
||||||
? 2 * atan2(n1,_Qreal(q1))
|
? 2 * atan2(n1,_Qreal(q1))
|
||||||
: 2 * acos(q1*q2/n1/norm(q2)) ;
|
: let( q1 = q1/norm(q1),
|
||||||
|
q2 = q2/norm(q2) )
|
||||||
|
4 * atan2(norm(q1 - q2), norm(q1 + q2));
|
||||||
|
|
||||||
// Function&Module: Qrot()
|
// Function&Module: Qrot()
|
||||||
// Usage: As Module
|
// Usage: As Module
|
||||||
|
@ -466,15 +465,15 @@ function Q_Rotation(R) =
|
||||||
: let( i = max_index([ R[0][0], R[1][1], R[2][2] ]),
|
: 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] )
|
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==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])) )
|
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])) );
|
_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])
|
// Function&Module: Q_Rotation_path(q1, n, [q2])
|
||||||
// Usage as a function:
|
// Usage: As a function
|
||||||
// path = Q_Rotation_path(q1, n, q2);
|
// path = Q_Rotation_path(q1, n, q2);
|
||||||
// path = Q_Rotation_path(q1, n);
|
// path = Q_Rotation_path(q1, n);
|
||||||
// Usage as a module:
|
// Usage: As a module
|
||||||
// Q_Rotation_path(q1, n, q2) ...
|
// Q_Rotation_path(q1, n, q2) ...
|
||||||
// Description:
|
// Description:
|
||||||
// If q2 is undef and it is called as a function, the path, with length n+1 (n>=1), will be the
|
// If q2 is undef and it is called as a function, the path, with length n+1 (n>=1), will be the
|
||||||
|
@ -516,12 +515,12 @@ function Q_Rotation(R) =
|
||||||
// #sphere(r=80);
|
// #sphere(r=80);
|
||||||
function Q_Rotation_path(q1, n=1, q2) =
|
function Q_Rotation_path(q1, n=1, q2) =
|
||||||
assert( Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2) ), "Invalid quaternion(s)" )
|
assert( Q_is_quat(q1) && (is_undef(q2) || Q_is_quat(q2) ), "Invalid quaternion(s)" )
|
||||||
assert( is_num(0*n) && n>=1 && n==floor(n), "Invalid integer" )
|
assert( is_finite(n) && n>=1 && n==floor(n), "Invalid integer" )
|
||||||
assert( is_undef(q2) || ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" )
|
assert( is_undef(q2) || ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" )
|
||||||
is_undef(q2)
|
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_Matrix4(q1), R=dR; i<=n; i=i+1, R=dR*R ) R]
|
||||||
: let( q2 = Q_Normalize( q1*q2<0 ? -q2: q2 ) )
|
: let( q2 = Q_Normalize( q1*q2<0 ? -q2: q2 ),
|
||||||
let( dq = Q_pow( Q_Mul( q2, Q_Inverse(q1) ), 1/n ),
|
dq = Q_pow( Q_Mul( q2, Q_Inverse(q1) ), 1/n ),
|
||||||
dR = Q_Matrix4(dq) )
|
dR = Q_Matrix4(dq) )
|
||||||
[for( i=0, R=Q_Matrix4(q1); i<=n; i=i+1, R=dR*R ) R];
|
[for( i=0, R=Q_Matrix4(q1); i<=n; i=i+1, R=dR*R ) R];
|
||||||
|
|
||||||
|
@ -559,15 +558,15 @@ module Q_Rotation_path(q1, n=1, q2) {
|
||||||
// Qrot(q) right(80) cube([10,10,1]);
|
// Qrot(q) right(80) cube([10,10,1]);
|
||||||
// #sphere(r=80);
|
// #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(Q_is_quat(q1) && Q_is_quat(q2), "Invalid quaternion(s)" )
|
||||||
assert( ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" )
|
assert( ! approx(norm(q1+q2),0), "Quaternions cannot be opposed" )
|
||||||
assert(is_num(0*u) || is_range(u) || (is_list(u) && is_num(0*u*u)) ,
|
|
||||||
"Invalid interpolation coefficient(s)" )
|
|
||||||
let( q1 = Q_Normalize(q1),
|
let( q1 = Q_Normalize(q1),
|
||||||
q2 = Q_Normalize(q2) )
|
q2 = Q_Normalize(q2) )
|
||||||
!is_num(u)
|
is_num(u)
|
||||||
? [for (ui=u) _Qnorm((1-ui)*q1 + ui*q2 ) ]
|
? _Qnorm((1-u)*q1 + u*q2 )
|
||||||
: _Qnorm((1-u)*q1 + u*q2 );
|
: [for (ui=u) _Qnorm((1-ui)*q1 + ui*q2 ) ];
|
||||||
|
|
||||||
|
|
||||||
// Function: Q_Squad()
|
// Function: Q_Squad()
|
||||||
|
@ -610,7 +609,7 @@ function Q_Nlerp(q1,q2,u) =
|
||||||
// Qrot(q) right(80) cube([10,10,1]);
|
// Qrot(q) right(80) cube([10,10,1]);
|
||||||
// #sphere(r=80);
|
// #sphere(r=80);
|
||||||
function Q_Squad(q1,q2,q3,q4,u) =
|
function Q_Squad(q1,q2,q3,q4,u) =
|
||||||
assert(is_num(0*u) || is_range(u) || (is_list(u) && is_num(0*u*u)) ,
|
assert(is_finite(u) || is_range(u) || is_vector(u) ,
|
||||||
"Invalid interpolation coefficient(s)" )
|
"Invalid interpolation coefficient(s)" )
|
||||||
is_num(u)
|
is_num(u)
|
||||||
? Q_Slerp( Q_Slerp(q1,q4,u), Q_Slerp(q2,q3,u), 2*u*(1-u))
|
? Q_Slerp( Q_Slerp(q1,q4,u), Q_Slerp(q2,q3,u), 2*u*(1-u))
|
||||||
|
@ -625,7 +624,7 @@ function Q_Squad(q1,q2,q3,q4,u) =
|
||||||
// The returned quaternion is usually not normalized.
|
// 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")
|
assert( is_vector(q,4), "Input is not a valid quaternion")
|
||||||
let( nv = norm(_Qvec(q)) ) // q may be equal to zero!
|
let( nv = norm(_Qvec(q)) ) // q may be equal to zero here!
|
||||||
exp(_Qreal(q))*Quat(_Qvec(q),2*nv);
|
exp(_Qreal(q))*Quat(_Qvec(q),2*nv);
|
||||||
|
|
||||||
|
|
||||||
|
@ -639,9 +638,8 @@ function Q_ln(q) =
|
||||||
assert(Q_is_quat(q), "Input is not a valid quaternion")
|
assert(Q_is_quat(q), "Input is not a valid quaternion")
|
||||||
let( nq = norm(q),
|
let( nq = norm(q),
|
||||||
nv = norm(_Qvec(q)) )
|
nv = norm(_Qvec(q)) )
|
||||||
approx(nv,0)
|
approx(nv,0) ? _Qset([0,0,0] , ln(nq) ) :
|
||||||
? _Qset([0,0,0] , ln(nq) )
|
_Qset(_Qvec(q)*atan2(nv,_Qreal(q))/nv, ln(nq));
|
||||||
: _Qset(_Qvec(q)*atan2(nv,_Qreal(q))/nv, ln(nq));
|
|
||||||
|
|
||||||
|
|
||||||
// Function: Q_pow()
|
// Function: Q_pow()
|
||||||
|
@ -651,11 +649,10 @@ function Q_ln(q) =
|
||||||
// Returns the quaternion that is the power of the quaternion q to the real exponent r.
|
// 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.
|
// The returned quaternion is normalized if `q` is normalized.
|
||||||
function Q_pow(q,r=1) =
|
function Q_pow(q,r=1) =
|
||||||
// Q_exp(r*Q_ln(q));
|
assert( Q_is_quat(q) && is_finite(r),
|
||||||
assert( Q_is_quat(q) && is_num(0*r),
|
|
||||||
"Invalid inputs")
|
"Invalid inputs")
|
||||||
let( theta = 2*atan2(norm(_Qvec(q)),_Qreal(q)) )
|
let( theta = 2*atan2(norm(_Qvec(q)),_Qreal(q)) )
|
||||||
Quat(_Qvec(q), r*theta);
|
Quat(_Qvec(q), r*theta); // Q_exp(r*Q_ln(q));
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue