From 851dec95acf53999a346ed92dac9609b2aa30de4 Mon Sep 17 00:00:00 2001 From: Revar Desmera Date: Fri, 1 Feb 2019 23:33:07 -0800 Subject: [PATCH] Added Q_Slerp() for spherical interpolation between two quaterions. --- quaternions.scad | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/quaternions.scad b/quaternions.scad index d16c141..0277ff2 100644 --- a/quaternions.scad +++ b/quaternions.scad @@ -31,6 +31,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +use + + // Quaternions are stored internally as a 4-value vector: // [X, Y, Z, W] = W + Xi + Yj + Zk function _Quat(a,s,w) = [a[0]*s, a[1]*s, a[2]*s, w]; @@ -60,6 +63,17 @@ function Q_Normalize(q) = q/Q_Norm(q); function Q_Dist(q1, q2) = Q_Norm(Q_Sub(q1-q2)); +// Returns a spherical interpolation between two quaternions. +function Q_Slerp(q1, q2, t) = let( + dot = Q_Dot(q1, q2), + qq2 = dot<0? Q_Neg(q2) : q2, + dott = dot<0? -dot : dot, + theta = t * acos(constrain(dott,-1,1)) + ) (dott>0.9995)? + Q_Normalize(Q_Add(q1, Q_Mul_S(Q_Sub(qq2,q1), t))) : + Q_Add(Q_Mul_S(q1,cos(theta)), Q_Mul_S(Q_Normalize(Q_Sub(qq2, Q_Mul_S(q1, dott))), sin(theta))); + + // Returns the 3x3 rotation matrix for the given normalized quaternion q. function Q_Matrix3(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]],