/*!
* Apply a rotation about an arbitrary axis to a matrix.
*
* \ingroup matrix
*/
void
lib3ds_matrix_rotate_axis(Lib3dsMatrix m, Lib3dsVector axis, Lib3dsFloat angle)
{
Lib3dsQuat q;
lib3ds_quat_axis_angle(q,axis,angle);
lib3ds_matrix_rotate(m,q);
}
/*!
* Apply a rotation about an arbitrary axis to a matrix.
*/
void
lib3ds_matrix_rotate(float m[4][4], float angle, float ax, float ay, float az) {
float q[4];
float axis[3];
lib3ds_vector_make(axis, ax, ay, az);
lib3ds_quat_axis_angle(q, axis, angle);
lib3ds_matrix_rotate_quat(m, q);
}
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 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);
}
}
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);
}
}
}