istereo
annotate libs/vmath/quat_c.c @ 28:c0ae8e668447
added vmath library
| author | John Tsiombikas <nuclear@mutantstargoat.com> |
|---|---|
| date | Thu, 08 Sep 2011 08:30:42 +0300 |
| parents | |
| children | ff055bff6a15 |
| rev | line source |
|---|---|
| nuclear@28 | 1 #include <stdio.h> |
| nuclear@28 | 2 #include <math.h> |
| nuclear@28 | 3 #include "quat.h" |
| nuclear@28 | 4 |
| nuclear@28 | 5 void quat_print(FILE *fp, quat_t q) |
| nuclear@28 | 6 { |
| nuclear@28 | 7 fprintf(fp, "([ %.4f %.4f %.4f ] %.4f)", q.x, q.y, q.z, q.w); |
| nuclear@28 | 8 } |
| nuclear@28 | 9 |
| nuclear@28 | 10 quat_t quat_rotate(quat_t q, scalar_t angle, scalar_t x, scalar_t y, scalar_t z) |
| nuclear@28 | 11 { |
| nuclear@28 | 12 quat_t rq; |
| nuclear@28 | 13 scalar_t half_angle = angle * 0.5; |
| nuclear@28 | 14 scalar_t sin_half = sin(half_angle); |
| nuclear@28 | 15 |
| nuclear@28 | 16 rq.w = cos(half_angle); |
| nuclear@28 | 17 rq.x = x * sin_half; |
| nuclear@28 | 18 rq.y = y * sin_half; |
| nuclear@28 | 19 rq.z = z * sin_half; |
| nuclear@28 | 20 |
| nuclear@28 | 21 return quat_mul(q, rq); |
| nuclear@28 | 22 } |
| nuclear@28 | 23 |
| nuclear@28 | 24 quat_t quat_rotate_quat(quat_t q, quat_t rotq) |
| nuclear@28 | 25 { |
| nuclear@28 | 26 return quat_mul(quat_mul(rotq, q), quat_conjugate(rotq)); |
| nuclear@28 | 27 } |
| nuclear@28 | 28 |
| nuclear@28 | 29 quat_t quat_slerp(quat_t q1, quat_t q2, scalar_t t) |
| nuclear@28 | 30 { |
| nuclear@28 | 31 quat_t res; |
| nuclear@28 | 32 scalar_t angle = acos(q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z); |
| nuclear@28 | 33 scalar_t a = sin((1.0f - t) * angle); |
| nuclear@28 | 34 scalar_t b = sin(t * angle); |
| nuclear@28 | 35 scalar_t c = sin(angle); |
| nuclear@28 | 36 |
| nuclear@28 | 37 res.x = (q1.x * a + q2.x * b) / c; |
| nuclear@28 | 38 res.y = (q1.y * a + q2.y * b) / c; |
| nuclear@28 | 39 res.z = (q1.z * a + q2.z * b) / c; |
| nuclear@28 | 40 res.w = (q1.w * a + q2.w * b) / c; |
| nuclear@28 | 41 return quat_normalize(res); |
| nuclear@28 | 42 } |