Fixed several bugs, imported math library.
[sphoot.git] / src / ecore / math.c
1 #include "math.h"
2
3 void set_vector(vector_t *v, float x, float y, float z, float w) {
4     v->c[0] = x;
5     v->c[1] = y;
6     v->c[2] = z;
7     v->c[3] = w;
8 }
9
10 void add_vectors(vector_t *v1, vector_t *v2, vector_t *result) {
11     result->c[0] = v1->c[0] + v2->c[0];
12     result->c[1] = v1->c[1] + v2->c[1];
13     result->c[2] = v1->c[2] + v2->c[2];
14     result->c[3] = v1->c[3] + v2->c[3];
15 }
16
17 void sub_vectors(vector_t *v1, vector_t *v2, vector_t *result) {
18     result->c[0] = v1->c[0] - v2->c[0];
19     result->c[1] = v1->c[1] - v2->c[1];
20     result->c[2] = v1->c[2] - v2->c[2];
21     result->c[3] = v1->c[3] - v2->c[3];
22 }
23
24 void mul_vector(vector_t *v, float s, vector_t *result) {
25     result->c[0] = v->c[0] * s;
26     result->c[1] = v->c[1] * s;
27     result->c[2] = v->c[2] * s;
28     result->c[3] = v->c[3] * s;
29 }
30
31 float dot_vectors(vector_t *v1, vector_t *v2) {
32     return v1->c[0]*v2->c[0] + v1->c[1]*v2->c[1]
33             + v1->c[2]*v2->c[2] + v1->c[3]*v2->c[3];
34 }
35
36 void cross_vectors(vector_t *v1, vector_t *v2, vector_t *result) {
37     float rx = v1->c[1]*v2->c[2] - v1->c[2]*v2->c[1];
38     float ry = v1->c[2]*v2->c[0] - v1->c[0]*v2->c[2];
39     float rz = v1->c[0]*v2->c[1] - v1->c[1]*v2->c[0];
40     result->c[0] = rx;
41     result->c[1] = ry;
42     result->c[2] = rz;
43 }
44
45 void project_vector(vector_t *onto, vector_t *v, vector_t *result) {
46     /*
47             cos = v . onto
48             cos = adj/hyp
49
50             
51     */
52     float c = dot_vectors(v, onto) / sqrt(dot_vectors(onto, onto));
53     mul_vector(onto, c, result);
54 }
55
56 void set_quaternion(vector_t *q, float x, float y, float z, float angle) {
57     float s = sin(angle/2);
58     float c = cos(angle/2);
59     q->c[0] = s*x;
60     q->c[1] = s*y;
61     q->c[2] = s*z;
62     q->c[3] = c;
63 }
64
65 void set_quaternion_aa(vector_t *q, vector_t *n, float angle) {
66     mul_vector(n, sin(angle/2), q);
67     q->c[3] = cos(angle/2);
68 }
69
70 void mul_quaternions(vector_t *q1, vector_t *q2, vector_t *result) {
71     /* result = [s1*s2 - v1.v2, s1*v2 + s2*v1 + v1*v2]. */
72
73     float s12 = q1->c[3]*q2->c[3];
74
75     vector_t v, v2;
76
77     vector_t qv1 = *q1;
78     vector_t qv2 = *q2;
79     qv1.c[3] = 0;
80     qv2.c[3] = 0;
81     
82     cross_vectors(&qv1, &qv2, &v2);
83     mul_vector(q1, q2->c[3], &v);
84     mul_vector(q2, q1->c[3], result);
85     
86     add_vectors(result, &v, result);
87     add_vectors(result, &v2, result);
88
89     result->c[3] = s12 - dot_vectors(&qv1, &qv2);
90 }
91
92 void conj_quaternion(vector_t *q, vector_t *result) {
93     mul_vector(q, -1, result);
94     result->c[3] *= -1;
95 }
96
97 void rotate_vector(vector_t *q, vector_t *v, vector_t *result) {
98     vector_t qt;
99     conj_quaternion(q, &qt);
100
101     /* Quaternions are associative, thankfully. So: */
102     mul_quaternions(v, &qt, &qt);
103     mul_quaternions(q, &qt, result);
104
105     /* And we end up with result = q*v*q^{conjugate}. */
106 }