/* * Spherical linear interpolation of unit quaternions with spins */ #include #define EPSILON 1.0E-6 /* a tiny number */ #define TRUE 1 #define FALSE 0 typedef struct { /* quaternion type */ double w, x, y, z; } Quaternion; slerp(alpha, a, b, q, spin) double alpha; /* interpolation parameter (0 to 1) */ Quaternion *a, *b; /* start and end unit quaternions */ Quaternion *q; /* output interpolated quaternion */ int spin; /* number of extra spin rotations */ { double beta; /* complementary interp parameter */ double theta; /* angle between A and B */ double sin_t, cos_t; /* sine, cosine of theta */ double phi; /* theta plus spins */ int bflip; /* use negation of B? */ /* cosine theta = dot product of A and B */ cos_t = a->x*b->x + a->y*b->y + a->z*b->z + a->w*b->w; /* if B is on opposite hemisphere from A, use -B instead */ if (cos_t < 0.0) { cos_t = -cos_t; bflip = TRUE; } else bflip = FALSE; /* if B is (within precision limits) the same as A, * just linear interpolate between A and B. * Can't do spins, since we don't know what direction to spin. */ if (1.0 - cos_t < EPSILON) { beta = 1.0 - alpha; } else { /* normal case */ theta = acos(cos_t); phi = theta + spin * M_PI; sin_t = sin(theta); beta = sin(theta - alpha*phi) / sin_t; alpha = sin(alpha*phi) / sin_t; } if (bflip) alpha = -alpha; /* interpolate */ q->x = beta*a->x + alpha*b->x; q->y = beta*a->y + alpha*b->y; q->z = beta*a->z + alpha*b->z; q->w = beta*a->w + alpha*b->w; }