Esempio n. 1
0
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);
}
Esempio n. 2
0
void quat_add_to(union quat *o, const union quat *q)
{
	union quat tmp;

	quat_add(&tmp, o, q);
	quat_copy(o, &tmp);
}
Esempio n. 3
0
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);
}
Esempio n. 4
0
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);
	}
}
Esempio n. 5
0
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];
    }
  }
}
Esempio n. 6
0
QuatP* qadd(const Quat qa, Quat qb) {
    quat_add(qa,qb,qb);
    return qb;
}