union quat *quat_lerp(union quat *qo, const union quat *qfrom, const union quat *qto, float t)
{
double cosom = quat_dot(qfrom, qto);
/* qto = qfrom or qto = -qfrom so no rotation to slerp */
if (cosom >= 1.0) {
quat_copy(qo, qfrom);
return qo;
}
/* adjust for shortest path */
union quat to1;
if (cosom < 0.0) {
to1.v.x = -qto->v.x;
to1.v.y = -qto->v.y;
to1.v.z = -qto->v.z;
to1.v.w = -qto->v.w;
} else {
quat_copy(&to1, qto);
}
double scale0 = 1.0 - t;
double scale1 = t;
/* calculate final values */
qo->v.x = scale0 * qfrom->v.x + scale1 * to1.v.x;
qo->v.y = scale0 * qfrom->v.y + scale1 * to1.v.y;
qo->v.z = scale0 * qfrom->v.z + scale1 * to1.v.z;
qo->v.w = scale0 * qfrom->v.w + scale1 * to1.v.w;
return qo;
}
void quat_slerp(AD_Quaternion *q1, AD_Quaternion *q2, float4 t, float4 spin, AD_Quaternion *q3)
// Spherical Linear Interpolation
{
float4 EPSILON = 1E-6f; // questo epsilon e' OK !
float4 alpha, cosalpha, k1, k2;
float4 sinalpha, anglespin;
float4 flip=1;
cosalpha=quat_dot(q1, q2); // prodotto scalare
// caso di rotazioni tra quaternioni "molto" vicini fra loro; in questo caso
// si ottengono coefficenti k1 e k2 pari allo sviluppo in serie troncato al
// termine di grado 1; praticamente si approssima sin(a*x)=a*sin(x) e nei
// rapporti (quando si e' a monte della formula) si ottiene semplificazione
// tra seni; questo permette di evitare l'instabilita' numerica
if ((1.0-fabs(cosalpha))<EPSILON)
{
k1=1.0f-t;
k2=t;
}
else
{
alpha=acosf(cosalpha);
sinalpha=1.0f/sinf(alpha);
anglespin=(float)(alpha+spin*M_PI);
k1=sinf(alpha-t*anglespin)*sinalpha;
k2=sinf(t*anglespin)*sinalpha;
}
q3->x = k1*q1->x + k2*q2->x;
q3->y = k1*q1->y + k2*q2->y;
q3->z = k1*q1->z + k2*q2->z;
q3->w = k1*q1->w + k2*q2->w;
}
void matrix3_from_quat(struct matrix3 *dst, const struct quat *q)
{
float norm = quat_dot(q, q);
float s = (norm > 0.0f) ? (2.0f/norm) : 0.0f;
float xx = q->x * q->x * s;
float yy = q->y * q->y * s;
float zz = q->z * q->z * s;
float xy = q->x * q->y * s;
float xz = q->x * q->z * s;
float yz = q->y * q->z * s;
float wx = q->w * q->x * s;
float wy = q->w * q->y * s;
float wz = q->w * q->z * s;
dst->x.x = 1.0f - (yy + zz);
dst->x.y = xy + wz;
dst->x.z = xz - wy;
dst->x.w = 0.0f;
dst->y.x = xy - wz;
dst->y.y = 1.0f - (xx + zz);
dst->y.z = yz + wx;
dst->y.w = 0.0f;
dst->z.x = xz + wy;
dst->z.y = yz - wx;
dst->z.z = 1.0f - (xx + yy);
dst->z.w = 0.0f;
vec3_zero(&dst->t);
}
void quat_logdif(AD_Quaternion *q1, AD_Quaternion *q2, AD_Quaternion *q3)
{
AD_Quaternion inv, dif;
float len, len1, s;
quat_inverse (q1, &inv);
quat_mul (&inv, q2, &dif);
len = sqrtf (dif.x*dif.x + dif.y*dif.y + dif.z*dif.z);
s = quat_dot (q1, q2);
if (s != 0.0) len1 = atanf (len / s); else len1 = (float)(M_PI/2.0f);
if (len != 0.0) len1 /= len;
q3->w = 0.0;
q3->x = dif.x * len1;
q3->y = dif.y * len1;
q3->z = dif.z * len1;
}
quat_t quat_inverse(quat_t quat, quat_t dest) {
float dot = quat_dot(quat,quat),
invDot = 1.0/dot;
if(!dest || quat == dest) {
quat[0] *= -invDot;
quat[1] *= -invDot;
quat[2] *= -invDot;
quat[3] *= invDot;
return quat;
}
dest[0] = -quat[0]*invDot;
dest[1] = -quat[1]*invDot;
dest[2] = -quat[2]*invDot;
dest[3] = quat[3]*invDot;
return dest;
}
union quat *quat_slerp(union quat *qo, const union quat *qfrom, const union quat *qto, float t)
{
/* calc cosine */
double cosom = quat_dot(qfrom, qto);
/* qto = qfrom or qto = -qfrom so no rotation to slerp */
if (cosom >= 1.0) {
quat_copy(qo, qfrom);
return qo;
}
/* adjust for shortest path */
union quat to1;
if (cosom < 0.0) {
cosom = -cosom;
to1.v.x = -qto->v.x;
to1.v.y = -qto->v.y;
to1.v.z = -qto->v.z;
to1.v.w = -qto->v.w;
} else {
quat_copy(&to1, qto);
}
/* calculate coefficients */
double scale0, scale1;
if (cosom < 0.99995) {
/* standard case (slerp) */
double omega = acos(cosom);
double sinom = sin(omega);
scale0 = sin((1.0 - t) * omega) / sinom;
scale1 = sin(t * omega) / sinom;
} else {
/* "from" and "to" quaternions are very close
* ... so we can do a linear interpolation
*/
scale0 = 1.0 - t;
scale1 = t;
}
/* calculate final values */
qo->v.x = scale0 * qfrom->v.x + scale1 * to1.v.x;
qo->v.y = scale0 * qfrom->v.y + scale1 * to1.v.y;
qo->v.z = scale0 * qfrom->v.z + scale1 * to1.v.z;
qo->v.w = scale0 * qfrom->v.w + scale1 * to1.v.w;
return qo;
}
quat
quat_slerp(const quat *q1, const quat *q2, float t)
{
float cos_theta = quat_dot(q1, q2);
float theta = acos(cos_theta);
float sin_theta = sin(theta);
if (sin_theta > 0.001f) {
float w1 = sin( (1.f-t) * theta) / sin_theta;
float w2 = sin(t * theta) / sin_theta;
quat q3, q4;
q3 = quat_scale(q1, w1);
q4 = quat_scale(q2, w2);
return quat_add(&q3, &q4);
} else {
return quat_lerp(q1, q2, t);
}
}
void quat_slerp(quat* q, quat* p, quat* out, float t){
if(t <= 0.0){
*out = *q;
return;
}
if(t >= 1.0){
*out = *q;
return;
}
float cos_omega = quat_dot(q, p);
float qa = q->angle;
float qx = q->axis.x;
float qy = q->axis.y;
float qz = q->axis.z;
if(cos_omega){
qa = -qa;
qx = -qx;
qy = -qy;
qz = -qz;
cos_omega = -cos_omega;
}
float k0, k1;
if(cos_omega > 0.9999){
k0 = 1.0 - t;
k1 = t;
} else {
float sin_omega = sqrt(1.0 - pow(cos_omega, 2));
float omega = atan2(sin_omega, cos_omega);
float one_over_sin_omega = 1.0 / sin_omega;
k0 = sin((1.0 - t) * omega) * one_over_sin_omega;
k1 = sin(t * omega) * one_over_sin_omega;
}
out->axis.x = k0 * q->axis.x + k1 * qx;
out->axis.y = k0 * q->axis.y + k1 * qy;
out->axis.z = k0 * q->axis.z + k1 * qz;
out->angle = k0 * q->angle + k1 * qa;
}
float quat_magnitude(const Quat q) {
float ret = (float)(sqrt(quat_dot(q, q)));
return ret;
}
double quat_quick_misorientation(double* q1, double* q2)
{
double t = quat_dot(q1, q2);
t = MIN(1, MAX(-1, t));
return 2 * t * t - 1;
}
double quat_size(double* q)
{
return sqrt(quat_dot(q, q));
}
float qdot(const Quat qa, const Quat qb) {
float dot;
quat_dot(qa,qb,&dot);
return dot;
}
