void quat_slerp(const Quat qa, const Quat qb, float t, Quat r) {
log_assert(t>=0);
log_assert(t<=1);
float flip = 1.0;
double cosine = qa[3]*qb[3] + qa[0]*qb[0] + qa[1]*qb[1] + qa[2]*qb[2];
if( cosine < 0 ) {
cosine = -cosine;
flip = -1.0;
}
Quat ua,ub;
if( (1.0 - cosine) < CUTE_EPSILON ) {
quat_mul1f(qa, t*flip, ua);
quat_mul1f(qb, 1-t, ub);
quat_add(ua, ub, r);
return;
}
float theta = (float)acos(cosine);
float sine = (float)sin(theta);
float beta = (float)sin((1-t)*theta) / sine;
float alpha = (float)sin(t*theta) / sine * flip;
quat_mul1f(qa, alpha, ua);
quat_mul1f(qb, beta, ub);
quat_add(ua, ub, r);
}
void quat_add_to(union quat *o, const union quat *q)
{
union quat tmp;
quat_add(&tmp, o, q);
quat_copy(o, &tmp);
}
quat
quat_lerp(const quat *q1, const quat *q2, float t)
{
quat q3, q4;
q3 = quat_scale(q1, (1-t));
q4 = quat_scale(q2, t);
q3 = quat_add(&q3, &q4);
return quat_normalize(&q3);
}
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 jedi_processInput_Acceleration(double acc[3], uint32_t accel_time) {
delta_time = accel_time;
const double curAcc =
pow(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2], 0.5);
if ((curAcc > gravity - ACC_THRESHOLD) &&
(curAcc < gravity + ACC_THRESHOLD)) {
quat *grav_quat = quat_fromGravityVector(acc);
global_quat = *quat_normalize(quat_add(&global_quat, grav_quat));
// fprintf(stderr, "global_quat %3.2f %3.2f %3.2f %3.2f\n", global_quat.W,
// global_quat.X, global_quat.Y, global_quat.Z);
free(grav_quat);
const double spdSqr = spd_hpf[0] * spd_hpf[0] + spd_hpf[1] * spd_hpf[1] +
spd_hpf[2] * spd_hpf[2];
if (spdSqr < ZERO_SPEED_THRESHOLD * ZERO_SPEED_THRESHOLD) {
spd[0] = 0;
spd[1] = 0;
spd[2] = 0;
spd_lpf[0] = 0;
spd_lpf[1] = 0;
spd_lpf[2] = 0;
spd_hpf[0] = 0;
spd_hpf[1] = 0;
spd_hpf[2] = 0;
if (action_done) {
printf("Ready for the next gesture.\n");
action_done = false;
}
}
vec3 rot_acc = quat_rotateVector(&global_quat, acc);
acc_hold[0] = 0.7 * acc_hold[0] + 0.3 * rot_acc.X;
acc_hold[1] = 0.7 * acc_hold[1] + 0.3 * rot_acc.Y;
acc_hold[2] = 0.7 * acc_hold[2] + 0.3 * rot_acc.Z;
for (int i = 0; i < 3; i++) {
spd[i] += acc_hold[i] * accel_time / 200.0;
spd_lpf[i] += (spd[i] - spd_lpf[i]) *
(1 - exp(-(double)accel_time / SPEED_LPF_TIME_CONSTANT));
spd_hpf[i] = spd[i] - spd_lpf[i];
}
}
}
QuatP* qadd(const Quat qa, Quat qb) {
quat_add(qa,qb,qb);
return qb;
}
