static void
setup_segment(Lib3dsTrack *track, int index, Lib3dsKey *pp, Lib3dsKey *p0, Lib3dsKey *p1, Lib3dsKey *pn) {
int ip, in;
pp->frame = pn->frame = -1;
if (index >= 2) {
ip = index - 2;
*pp = track->keys[index - 2];
} else {
if (track->flags & LIB3DS_TRACK_SMOOTH) {
ip = track->nkeys - 2;
*pp = track->keys[track->nkeys - 2];
pp->frame = track->keys[track->nkeys - 2].frame - (track->keys[track->nkeys - 1].frame - track->keys[0].frame);
}
else ip = -1; // Avoids a compiler warning
}
*p0 = track->keys[index - 1];
*p1 = track->keys[index];
if (index < (int)track->nkeys - 1) {
in = index + 1;
*pn = track->keys[index + 1];
} else {
if (track->flags & LIB3DS_TRACK_SMOOTH) {
in = 1;
*pn = track->keys[1];
pn->frame = track->keys[1].frame + (track->keys[track->nkeys-1].frame - track->keys[0].frame);
}
else in = -1; // Avoids a compiler warning
}
if (track->type == LIB3DS_TRACK_QUAT) {
float q[4];
if (pp->frame >= 0) {
quat_for_index(track, ip, pp->value);
} else {
lib3ds_quat_identity(pp->value);
}
quat_for_index(track, index - 1, p0->value);
lib3ds_quat_axis_angle(q, track->keys[index].value, track->keys[index].value[3]);
lib3ds_quat_mul(p1->value, q, p0->value);
if (pn->frame >= 0) {
lib3ds_quat_axis_angle(q, track->keys[in].value, track->keys[in].value[3]);
lib3ds_quat_mul(pn->value, q, p1->value);
} else {
lib3ds_quat_identity(pn->value);
}
}
}
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);
}
static void
quat_for_index(Lib3dsTrack *track, int index, float q[4]) {
float p[4];
int i;
lib3ds_quat_identity(q);
for (i = 0; i <= index; ++i) {
lib3ds_quat_axis_angle(p, track->keys[i].value, track->keys[i].value[3]);
lib3ds_quat_mul(q, p, q);
}
}
/*!
* \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);
}
/*!
* \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);
}
}
void
lib3ds_quat_tangent(float c[4], float p[4], float q[4], float n[4]) {
float dn[4], dp[4], x[4];
int i;
lib3ds_quat_ln_dif(dn, q, n);
lib3ds_quat_ln_dif(dp, q, p);
for (i = 0; i < 4; i++) {
x[i] = -1.0f / 4.0f * (dn[i] + dp[i]);
}
lib3ds_quat_exp(x);
lib3ds_quat_mul(c, q, x);
}
/*!
* \ingroup quat
*/
void
lib3ds_quat_tangent(Lib3dsQuat c, Lib3dsQuat p, Lib3dsQuat q, Lib3dsQuat n)
{
Lib3dsQuat dn,dp,x;
int i;
lib3ds_quat_ln_dif(dn, q, n);
lib3ds_quat_ln_dif(dp, q, p);
for (i=0; i<4; i++) {
x[i]=-1.0f/4.0f*(dn[i]+dp[i]);
}
lib3ds_quat_exp(x);
lib3ds_quat_mul(c,q,x);
}
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);
}
}
}