|
|
1 | /* |
|
|
2 | * math support |
|
|
3 | * most of the more complicated code is taken from mesa. |
|
|
4 | */ |
|
|
5 | |
| 1 | #include <cstdio> // ugly |
6 | #include <cstdio> // ugly |
| 2 | |
7 | |
| 3 | #include <cmath> |
8 | #include <cmath> |
| 4 | |
9 | |
| 5 | #include <blitz/tinymat.h> |
10 | #include <blitz/tinymat.h> |
| 6 | |
11 | |
| 7 | using namespace blitz; |
12 | using namespace blitz; |
| 8 | |
13 | |
| 9 | #include "util.h" |
14 | #include "util.h" |
| 10 | |
15 | |
|
|
16 | #define DEG2RAD (M_PI / 180.) |
|
|
17 | |
| 11 | const vec3 normalize (const vec3 &v) |
18 | const vec3 normalize (const vec3 &v) |
| 12 | { |
19 | { |
| 13 | GLfloat s = 1. / sqrtf (v.x * v.x + v.y * v.y + v.z * v.z); |
20 | GLfloat s = abs (v); |
| 14 | |
21 | |
|
|
22 | if (!s) |
|
|
23 | return v; |
|
|
24 | |
|
|
25 | s = 1. / s; |
| 15 | return vec3 (v.x * s, v.y * s, v.z * s); |
26 | return vec3 (v.x * s, v.y * s, v.z * s); |
| 16 | } |
27 | } |
| 17 | |
28 | |
| 18 | const vec3 cross (const vec3 &a, const vec3 &b) |
29 | const vec3 cross (const vec3 &a, const vec3 &b) |
| 19 | { |
30 | { |
| 20 | return vec3 ( |
31 | return vec3 ( |
| 21 | a.y * b.z - a.z * b.y, |
32 | a.y * b.z - a.z * b.y, |
| 22 | a.z * b.x - a.x * b.z, |
33 | a.z * b.x - a.x * b.z, |
| 23 | a.x * b.y - a.y * b.x |
34 | a.x * b.y - a.y * b.x |
| 24 | ); |
35 | ); |
| 25 | } |
|
|
| 26 | |
|
|
| 27 | GLfloat dot (const vec3 &a, const vec3 &b) |
|
|
| 28 | { |
|
|
| 29 | return a.x * b.x + a.y * b.y + a.z * b.z; |
|
|
| 30 | } |
36 | } |
| 31 | |
37 | |
| 32 | void gl_matrix::diagonal (GLfloat v) |
38 | void gl_matrix::diagonal (GLfloat v) |
| 33 | { |
39 | { |
| 34 | for (int i = 4; i--; ) |
40 | for (int i = 4; i--; ) |
| … | |
… | |
| 38 | |
44 | |
| 39 | const gl_matrix operator *(const gl_matrix &a, const gl_matrix &b) |
45 | const gl_matrix operator *(const gl_matrix &a, const gl_matrix &b) |
| 40 | { |
46 | { |
| 41 | gl_matrix r; |
47 | gl_matrix r; |
| 42 | |
48 | |
| 43 | int i, j, k; |
49 | // taken from mesa |
| 44 | |
|
|
| 45 | for (i = 0; i < 4; i++) |
50 | for (int i = 0; i < 4; i++) |
| 46 | for (j = 0; j < 4; j++) |
|
|
| 47 | { |
51 | { |
| 48 | GLfloat s = 0.; |
52 | const GLfloat ai0=a(i,0), ai1=a(i,1), ai2=a(i,2), ai3=a(i,3); |
| 49 | for (k = 0; k < 4; k++) |
|
|
| 50 | s += a(i,k) * b(k,j); |
|
|
| 51 | |
53 | |
| 52 | r(i,j) = s; |
54 | r(i,0) = ai0 * b(0,0) + ai1 * b(1,0) + ai2 * b(2,0) + ai3 * b(3,0); |
|
|
55 | r(i,1) = ai0 * b(0,1) + ai1 * b(1,1) + ai2 * b(2,1) + ai3 * b(3,1); |
|
|
56 | r(i,2) = ai0 * b(0,2) + ai1 * b(1,2) + ai2 * b(2,2) + ai3 * b(3,2); |
|
|
57 | r(i,3) = ai0 * b(0,3) + ai1 * b(1,3) + ai2 * b(2,3) + ai3 * b(3,3); |
| 53 | } |
58 | } |
| 54 | |
59 | |
| 55 | return r; |
60 | return r; |
|
|
61 | } |
|
|
62 | |
|
|
63 | void gl_matrix::rotate (GLfloat angle, const vec3 &axis) |
|
|
64 | { |
|
|
65 | gl_matrix m; |
|
|
66 | GLfloat xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c; |
|
|
67 | |
|
|
68 | s = (GLfloat) sinf (angle * DEG2RAD); |
|
|
69 | c = (GLfloat) cosf (angle * DEG2RAD); |
|
|
70 | |
|
|
71 | const GLfloat mag = abs (axis); |
|
|
72 | |
|
|
73 | if (mag <= 1.0e-4) |
|
|
74 | return; /* no rotation, leave mat as-is */ |
|
|
75 | |
|
|
76 | const vec3 n = axis * (1. / mag); |
|
|
77 | |
|
|
78 | /* |
|
|
79 | * Arbitrary axis rotation matrix. |
|
|
80 | * |
|
|
81 | * This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied |
|
|
82 | * like so: Rz * Ry * T * Ry' * Rz'. T is the final rotation |
|
|
83 | * (which is about the X-axis), and the two composite transforms |
|
|
84 | * Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary |
|
|
85 | * from the arbitrary axis to the X-axis then back. They are |
|
|
86 | * all elementary rotations. |
|
|
87 | * |
|
|
88 | * Rz' is a rotation about the Z-axis, to bring the axis vector |
|
|
89 | * into the x-z plane. Then Ry' is applied, rotating about the |
|
|
90 | * Y-axis to bring the axis vector parallel with the X-axis. The |
|
|
91 | * rotation about the X-axis is then performed. Ry and Rz are |
|
|
92 | * simply the respective inverse transforms to bring the arbitrary |
|
|
93 | * axis back to it's original orientation. The first transforms |
|
|
94 | * Rz' and Ry' are considered inverses, since the data from the |
|
|
95 | * arbitrary axis gives you info on how to get to it, not how |
|
|
96 | * to get away from it, and an inverse must be applied. |
|
|
97 | * |
|
|
98 | * The basic calculation used is to recognize that the arbitrary |
|
|
99 | * axis vector (x, y, z), since it is of unit length, actually |
|
|
100 | * represents the sines and cosines of the angles to rotate the |
|
|
101 | * X-axis to the same orientation, with theta being the angle about |
|
|
102 | * Z and phi the angle about Y (in the order described above) |
|
|
103 | * as follows: |
|
|
104 | * |
|
|
105 | * cos ( theta ) = x / sqrt ( 1 - z^2 ) |
|
|
106 | * sin ( theta ) = y / sqrt ( 1 - z^2 ) |
|
|
107 | * |
|
|
108 | * cos ( phi ) = sqrt ( 1 - z^2 ) |
|
|
109 | * sin ( phi ) = z |
|
|
110 | * |
|
|
111 | * Note that cos ( phi ) can further be inserted to the above |
|
|
112 | * formulas: |
|
|
113 | * |
|
|
114 | * cos ( theta ) = x / cos ( phi ) |
|
|
115 | * sin ( theta ) = y / sin ( phi ) |
|
|
116 | * |
|
|
117 | * ...etc. Because of those relations and the standard trigonometric |
|
|
118 | * relations, it is pssible to reduce the transforms down to what |
|
|
119 | * is used below. It may be that any primary axis chosen will give the |
|
|
120 | * same results (modulo a sign convention) using this method. |
|
|
121 | * |
|
|
122 | * Particularly nice is to notice that all divisions that might |
|
|
123 | * have caused trouble when parallel to certain planes or |
|
|
124 | * axis go away with care paid to reducing the expressions. |
|
|
125 | * After checking, it does perform correctly under all cases, since |
|
|
126 | * in all the cases of division where the denominator would have |
|
|
127 | * been zero, the numerator would have been zero as well, giving |
|
|
128 | * the expected result. |
|
|
129 | */ |
|
|
130 | |
|
|
131 | xx = n.x * n.x; |
|
|
132 | yy = n.y * n.y; |
|
|
133 | zz = n.z * n.z; |
|
|
134 | xy = n.x * n.y; |
|
|
135 | yz = n.y * n.z; |
|
|
136 | zx = n.z * n.x; |
|
|
137 | xs = n.x * s; |
|
|
138 | ys = n.y * s; |
|
|
139 | zs = n.z * s; |
|
|
140 | one_c = 1.0F - c; |
|
|
141 | |
|
|
142 | m(0,0) = (one_c * xx) + c; |
|
|
143 | m(0,1) = (one_c * xy) - zs; |
|
|
144 | m(0,2) = (one_c * zx) + ys; |
|
|
145 | m(0,3) = 0; |
|
|
146 | |
|
|
147 | m(1,0) = (one_c * xy) + zs; |
|
|
148 | m(1,1) = (one_c * yy) + c; |
|
|
149 | m(1,2) = (one_c * yz) - xs; |
|
|
150 | m(1,3) = 0; |
|
|
151 | |
|
|
152 | m(2,0) = (one_c * zx) - ys; |
|
|
153 | m(2,1) = (one_c * yz) + xs; |
|
|
154 | m(2,2) = (one_c * zz) + c; |
|
|
155 | m(2,3) = 0; |
|
|
156 | |
|
|
157 | m(3,0) = 0; |
|
|
158 | m(3,1) = 0; |
|
|
159 | m(3,2) = 0; |
|
|
160 | m(3,3) = 1; |
|
|
161 | |
|
|
162 | (*this) = (*this) * m; |
|
|
163 | } |
|
|
164 | |
|
|
165 | const vec3 operator *(const gl_matrix &a, const vec3 &v) |
|
|
166 | { |
|
|
167 | return vec3 ( |
|
|
168 | a(0,0) * v.x + a(1,0) * v.y + a(2,0) * v.z + a(3,0), |
|
|
169 | a(0,1) * v.x + a(1,1) * v.y + a(2,1) * v.z + a(3,1), |
|
|
170 | a(0,2) * v.x + a(1,2) * v.y + a(2,2) * v.z + a(3,2) |
|
|
171 | ); |
| 56 | } |
172 | } |
| 57 | |
173 | |
| 58 | void gl_matrix::print () |
174 | void gl_matrix::print () |
| 59 | { |
175 | { |
| 60 | printf ("\n"); |
176 | printf ("\n"); |
