1 /* some geometric routines always needed */
2
3 #include <math.h>
4
length(float * v)5 float length(float *v) {
6 return (float)sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
7 }
8
length2(float * v)9 float length2(float *v) {
10 return (float)sqrt(v[0] * v[0] + v[1] * v[1]);
11 }
12
length3(float * v)13 float length3(float *v) {
14 return length(v);
15 }
16
normalize(float * v)17 void normalize(float *v) {
18 float d = length(v);
19 if (d == 0) return;
20 v[0] /= d;
21 v[1] /= d;
22 v[2] /= d;
23 }
24
crossprod(float * v1,float * v2,float * out)25 void crossprod(float *v1, float *v2, float *out) {
26 out[0] = v1[1] * v2[2] - v1[2] * v2[1];
27 out[1] = v1[2] * v2[0] - v1[0] * v2[2];
28 out[2] = v1[0] * v2[1] - v1[1] * v2[0];
29 }
30
normcrossprod(float * v1,float * v2,float * out)31 void normcrossprod(float *v1, float *v2, float *out) {
32 crossprod(v1, v2, out);
33 normalize(out);
34 }
35
scalarprod2(float * v1,float * v2)36 float scalarprod2(float *v1, float *v2) {
37 return v1[0] * v2[0] + v1[1] * v2[1];
38 }
39
scalarprod(float * v1,float * v2)40 float scalarprod(float *v1, float *v2) {
41 return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
42 }
43
vsub2(float * v1,float * v2,float * out)44 void vsub2(float *v1, float *v2, float *out) {
45 out[0] = v1[0] - v2[0];
46 out[1] = v1[1] - v2[1];
47 }
48
vsub(float * v1,float * v2,float * out)49 void vsub(float *v1, float *v2, float *out) {
50 out[0] = v1[0] - v2[0];
51 out[1] = v1[1] - v2[1];
52 out[2] = v1[2] - v2[2];
53 }
54
vadd2(float * v1,float * v2,float * out)55 void vadd2(float *v1, float *v2, float *out) {
56 out[0] = v1[0] + v2[0];
57 out[1] = v1[1] + v2[1];
58 }
59
vadd(float * v1,float * v2,float * out)60 void vadd(float *v1, float *v2, float *out) {
61 out[0] = v1[0] + v2[0];
62 out[1] = v1[1] + v2[1];
63 out[2] = v1[2] + v2[2];
64 }
65
vcopy(float * v1,float * out)66 void vcopy(float *v1, float *out) {
67 out[0] = v1[0];
68 out[1] = v1[1];
69 out[2] = v1[2];
70 }
71
vmul(float * v,float f)72 void vmul(float *v, float f) {
73 v[0] *= f;
74 v[1] *= f;
75 v[2] *= f;
76 }
77
78 /* 4 entries... */
79
length4(float * v)80 float length4(float *v) {
81 return (float)sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] +
82 v[3] * v[3]);
83 }
84
normalize4(float * v)85 void normalize4(float *v) {
86 float d = length(v);
87 if (d == 0) return;
88 v[0] /= d;
89 v[1] /= d;
90 v[2] /= d;
91 v[3] /= d;
92 }
93
scalarprod4(float * v1,float * v2)94 float scalarprod4(float *v1, float *v2) {
95 return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2] +
96 v1[3] * v2[3];
97 }
98
vsub4(float * v1,float * v2,float * out)99 void vsub4(float *v1, float *v2, float *out) {
100 out[0] = v1[0] - v2[0];
101 out[1] = v1[1] - v2[1];
102 out[2] = v1[2] - v2[2];
103 out[3] = v1[3] - v2[3];
104 }
105
vadd4(float * v1,float * v2,float * out)106 void vadd4(float *v1, float *v2, float *out) {
107 out[0] = v1[0] + v2[0];
108 out[1] = v1[1] + v2[1];
109 out[2] = v1[2] + v2[2];
110 out[3] = v1[3] + v2[3];
111 }
112
vcopy4(float * v1,float * out)113 void vcopy4(float *v1, float *out) {
114 out[0] = v1[0];
115 out[1] = v1[1];
116 out[2] = v1[2];
117 out[3] = v1[3];
118 }
119