/*!
* Multiply two quaternions.
*
* \param c Result
* \param a,b Inputs
*/
void
lib3ds_quat_mul(float c[4], float a[4], float b[4]) {
float qa[4], qb[4];
lib3ds_quat_copy(qa, a);
lib3ds_quat_copy(qb, b);
c[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] - qa[2] * qb[1];
c[1] = qa[3] * qb[1] + qa[1] * qb[3] + qa[2] * qb[0] - qa[0] * qb[2];
c[2] = qa[3] * qb[2] + qa[2] * qb[3] + qa[0] * qb[1] - qa[1] * qb[0];
c[3] = qa[3] * qb[3] - qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2];
}
/*!
* \ingroup tracks
*/
void
lib3ds_quat_track_setup(Lib3dsQuatTrack *track)
{
Lib3dsQuatKey *pp,*pc,*pn,*pl;
Lib3dsQuat q;
ASSERT(track);
for (pp=0,pc=track->keyL; pc; pp=pc,pc=pc->next) {
lib3ds_quat_axis_angle(q, pc->axis, pc->angle);
if (pp) {
lib3ds_quat_mul(pc->q, q, pp->q);
}
else {
lib3ds_quat_copy(pc->q, q);
}
}
pc=track->keyL;
if (!pc) {
return;
}
if (!pc->next) {
lib3ds_quat_copy(pc->ds, pc->q);
lib3ds_quat_copy(pc->dd, pc->q);
return;
}
if (track->flags&LIB3DS_SMOOTH) {
for (pl=track->keyL; pl->next->next; pl=pl->next);
lib3ds_quat_key_setup(pl, pl->next, pc, 0, pc->next);
}
else {
lib3ds_quat_key_setup(0, 0, pc, 0, pc->next);
}
for (;;) {
pp=pc;
pc=pc->next;
pn=pc->next;
if (!pn) {
break;
}
lib3ds_quat_key_setup(pp, 0, pc, 0, pn);
}
if (track->flags&LIB3DS_SMOOTH) {
lib3ds_quat_key_setup(pp, 0, pc, track->keyL, track->keyL->next);
}
else {
lib3ds_quat_key_setup(pp, 0, pc, 0, 0);
}
}
/*!
* \ingroup tracks
*/
void
lib3ds_quat_track_eval(Lib3dsQuatTrack *track, Lib3dsQuat q, Lib3dsFloat t)
{
Lib3dsQuatKey *k;
Lib3dsFloat nt;
Lib3dsFloat u;
if (!track->keyL) {
lib3ds_quat_identity(q);
return;
}
if (!track->keyL->next) {
lib3ds_quat_copy(q, track->keyL->q);
return;
}
for (k=track->keyL; k->next!=0; k=k->next) {
if ((t>=k->tcb.frame) && (t<k->next->tcb.frame)) {
break;
}
}
if (!k->next) {
if (track->flags&LIB3DS_REPEAT) {
nt=(Lib3dsFloat)fmod(t, k->tcb.frame);
for (k=track->keyL; k->next!=0; k=k->next) {
if ((nt>=k->tcb.frame) && (nt<k->next->tcb.frame)) {
break;
}
}
ASSERT(k->next);
}
else {
lib3ds_quat_copy(q, k->q);
return;
}
}
else {
nt=t;
}
u=nt - k->tcb.frame;
u/=(k->next->tcb.frame - k->tcb.frame);
lib3ds_quat_squad(
q,
k->q,
k->dd,
k->next->ds,
k->next->q,
u
);
}
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);
}
}
}