void
lib3ds_quat_ln_dif(float c[4], float a[4], float b[4]) {
float invp[4];
lib3ds_quat_copy(invp, a);
lib3ds_quat_inv(invp);
lib3ds_quat_mul(c, invp, b);
lib3ds_quat_ln(c);
}
/*!
* \ingroup quat
*/
void
lib3ds_quat_ln_dif(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b)
{
Lib3dsQuat invp;
lib3ds_quat_copy(invp, a);
lib3ds_quat_inv(invp);
lib3ds_quat_mul(c, invp, b);
lib3ds_quat_ln(c);
}
static void
rot_key_setup(Lib3dsKey *prev, Lib3dsKey *cur, Lib3dsKey *next, float a[4], float b[4]) {
float tm, cm, cp, bm, bp, tmcm, tmcp, ksm, ksp, kdm, kdp, c;
float dt, fp, fn;
float q[4], qm[4], qp[4], qa[4], qb[4];
int i;
assert(cur);
if (prev) {
if (cur->value[3] > LIB3DS_TWOPI - LIB3DS_EPSILON) {
lib3ds_quat_axis_angle(qm, cur->value, 0.0f);
lib3ds_quat_ln(qm);
} else {
lib3ds_quat_copy(q, prev->value);
if (lib3ds_quat_dot(q, cur->value) < 0) lib3ds_quat_neg(q);
lib3ds_quat_ln_dif(qm, q, cur->value);
}
}
if (next) {
if (next->value[3] > LIB3DS_TWOPI - LIB3DS_EPSILON) {
lib3ds_quat_axis_angle(qp, next->value, 0.0f);
lib3ds_quat_ln(qp);
} else {
lib3ds_quat_copy(q, next->value);
if (lib3ds_quat_dot(q, cur->value) < 0) lib3ds_quat_neg(q);
lib3ds_quat_ln_dif(qp, cur->value, q);
}
}
if (!prev) lib3ds_quat_copy(qm, qp);
if (!next) lib3ds_quat_copy(qp, qm);
fp = fn = 1.0f;
cm = 1.0f - cur->cont;
if (prev && next) {
dt = 0.5f * (next->frame - prev->frame);
fp = (float)(cur->frame - prev->frame) / dt;
fn = (float)(next->frame - cur->frame) / dt;
c = (float)fabs(cur->cont);
fp = fp + c - c * fp;
fn = fn + c - c * fn;
}
tm = 0.5f * (1.0f - cur->tens);
cp = 2.0f - cm;
bm = 1.0f - cur->bias;
bp = 2.0f - bm;
tmcm = tm * cm;
tmcp = tm * cp;
ksm = 1.0f - tmcm * bp * fp;
ksp = -tmcp * bm * fp;
kdm = tmcp * bp * fn;
kdp = tmcm * bm * fn - 1.0f;
for (i = 0; i < 4; i++) {
qa[i] = 0.5f * (kdm * qm[i] + kdp * qp[i]);
qb[i] = 0.5f * (ksm * qm[i] + ksp * qp[i]);
}
lib3ds_quat_exp(qa);
lib3ds_quat_exp(qb);
lib3ds_quat_mul(a, cur->value, qa);
lib3ds_quat_mul(b, cur->value, qb);
}
/*!
* \ingroup tracks
*/
void
lib3ds_quat_key_setup(Lib3dsQuatKey *p, Lib3dsQuatKey *cp, Lib3dsQuatKey *c,
Lib3dsQuatKey *cn, Lib3dsQuatKey *n)
{
Lib3dsFloat ksm,ksp,kdm,kdp;
Lib3dsQuat q,qp,qn,qa,qb;
int i;
ASSERT(c);
if (!cp) {
cp=c;
}
if (!cn) {
cn=c;
}
if (!p || !n) {
lib3ds_quat_copy(c->ds, c->q);
lib3ds_quat_copy(c->dd, c->q);
return;
}
if (p) {
if (p->angle>LIB3DS_TWOPI-LIB3DS_EPSILON) {
lib3ds_quat_axis_angle(qp, p->axis, 0.0f);
lib3ds_quat_ln(qp);
}
else {
lib3ds_quat_copy(q, p->q);
if (lib3ds_quat_dot(q,c->q)<0) lib3ds_quat_neg(q);
lib3ds_quat_ln_dif(qp, c->q, q);
}
}
if (n) {
if (n->angle>LIB3DS_TWOPI-LIB3DS_EPSILON) {
lib3ds_quat_axis_angle(qn, n->axis, 0.0f);
lib3ds_quat_ln(qn);
}
else {
lib3ds_quat_copy(q, n->q);
if (lib3ds_quat_dot(q,c->q)<0) lib3ds_quat_neg(q);
lib3ds_quat_ln_dif(qn, c->q, q);
}
}
if (n && p) {
lib3ds_tcb(&p->tcb, &cp->tcb, &c->tcb, &cn->tcb, &n->tcb, &ksm, &ksp, &kdm, &kdp);
for(i=0; i<4; i++) {
qa[i]=-0.5f*(kdm*qn[i]+kdp*qp[i]);
qb[i]=-0.5f*(ksm*qn[i]+ksp*qp[i]);
}
lib3ds_quat_exp(qa);
lib3ds_quat_exp(qb);
lib3ds_quat_mul(c->ds, c->q, qa);
lib3ds_quat_mul(c->dd, c->q, qb);
}
else {
if (p) {
lib3ds_quat_exp(qp);
lib3ds_quat_mul(c->ds, c->q, qp);
lib3ds_quat_mul(c->dd, c->q, qp);
}
if (n) {
lib3ds_quat_exp(qn);
lib3ds_quat_mul(c->ds, c->q, qn);
lib3ds_quat_mul(c->dd, c->q, qn);
}
}
}