1 #ifndef LINMATH_H
2 #define LINMATH_H
3
4 #include <math.h>
5
6 #ifdef _MSC_VER
7 #define inline __inline
8 #endif
9
10 #define LINMATH_H_DEFINE_VEC(n) \
11 typedef float vec##n[n]; \
12 static inline void vec##n##_add(vec##n r, vec##n const a, vec##n const b) \
13 { \
14 int i; \
15 for(i=0; i<n; ++i) \
16 r[i] = a[i] + b[i]; \
17 } \
18 static inline void vec##n##_sub(vec##n r, vec##n const a, vec##n const b) \
19 { \
20 int i; \
21 for(i=0; i<n; ++i) \
22 r[i] = a[i] - b[i]; \
23 } \
24 static inline void vec##n##_scale(vec##n r, vec##n const v, float const s) \
25 { \
26 int i; \
27 for(i=0; i<n; ++i) \
28 r[i] = v[i] * s; \
29 } \
30 static inline float vec##n##_mul_inner(vec##n const a, vec##n const b) \
31 { \
32 float p = 0.; \
33 int i; \
34 for(i=0; i<n; ++i) \
35 p += b[i]*a[i]; \
36 return p; \
37 } \
38 static inline float vec##n##_len(vec##n const v) \
39 { \
40 return (float) sqrt(vec##n##_mul_inner(v,v)); \
41 } \
42 static inline void vec##n##_norm(vec##n r, vec##n const v) \
43 { \
44 float k = 1.f / vec##n##_len(v); \
45 vec##n##_scale(r, v, k); \
46 }
47
48 LINMATH_H_DEFINE_VEC(2)
49 LINMATH_H_DEFINE_VEC(3)
50 LINMATH_H_DEFINE_VEC(4)
51
vec3_mul_cross(vec3 r,vec3 const a,vec3 const b)52 static inline void vec3_mul_cross(vec3 r, vec3 const a, vec3 const b)
53 {
54 r[0] = a[1]*b[2] - a[2]*b[1];
55 r[1] = a[2]*b[0] - a[0]*b[2];
56 r[2] = a[0]*b[1] - a[1]*b[0];
57 }
58
vec3_reflect(vec3 r,vec3 const v,vec3 const n)59 static inline void vec3_reflect(vec3 r, vec3 const v, vec3 const n)
60 {
61 float p = 2.f*vec3_mul_inner(v, n);
62 int i;
63 for(i=0;i<3;++i)
64 r[i] = v[i] - p*n[i];
65 }
66
vec4_mul_cross(vec4 r,vec4 a,vec4 b)67 static inline void vec4_mul_cross(vec4 r, vec4 a, vec4 b)
68 {
69 r[0] = a[1]*b[2] - a[2]*b[1];
70 r[1] = a[2]*b[0] - a[0]*b[2];
71 r[2] = a[0]*b[1] - a[1]*b[0];
72 r[3] = 1.f;
73 }
74
vec4_reflect(vec4 r,vec4 v,vec4 n)75 static inline void vec4_reflect(vec4 r, vec4 v, vec4 n)
76 {
77 float p = 2.f*vec4_mul_inner(v, n);
78 int i;
79 for(i=0;i<4;++i)
80 r[i] = v[i] - p*n[i];
81 }
82
83 typedef vec4 mat4x4[4];
mat4x4_identity(mat4x4 M)84 static inline void mat4x4_identity(mat4x4 M)
85 {
86 int i, j;
87 for(i=0; i<4; ++i)
88 for(j=0; j<4; ++j)
89 M[i][j] = i==j ? 1.f : 0.f;
90 }
mat4x4_dup(mat4x4 M,mat4x4 N)91 static inline void mat4x4_dup(mat4x4 M, mat4x4 N)
92 {
93 int i, j;
94 for(i=0; i<4; ++i)
95 for(j=0; j<4; ++j)
96 M[i][j] = N[i][j];
97 }
mat4x4_row(vec4 r,mat4x4 M,int i)98 static inline void mat4x4_row(vec4 r, mat4x4 M, int i)
99 {
100 int k;
101 for(k=0; k<4; ++k)
102 r[k] = M[k][i];
103 }
mat4x4_col(vec4 r,mat4x4 M,int i)104 static inline void mat4x4_col(vec4 r, mat4x4 M, int i)
105 {
106 int k;
107 for(k=0; k<4; ++k)
108 r[k] = M[i][k];
109 }
mat4x4_transpose(mat4x4 M,mat4x4 N)110 static inline void mat4x4_transpose(mat4x4 M, mat4x4 N)
111 {
112 int i, j;
113 for(j=0; j<4; ++j)
114 for(i=0; i<4; ++i)
115 M[i][j] = N[j][i];
116 }
mat4x4_add(mat4x4 M,mat4x4 a,mat4x4 b)117 static inline void mat4x4_add(mat4x4 M, mat4x4 a, mat4x4 b)
118 {
119 int i;
120 for(i=0; i<4; ++i)
121 vec4_add(M[i], a[i], b[i]);
122 }
mat4x4_sub(mat4x4 M,mat4x4 a,mat4x4 b)123 static inline void mat4x4_sub(mat4x4 M, mat4x4 a, mat4x4 b)
124 {
125 int i;
126 for(i=0; i<4; ++i)
127 vec4_sub(M[i], a[i], b[i]);
128 }
mat4x4_scale(mat4x4 M,mat4x4 a,float k)129 static inline void mat4x4_scale(mat4x4 M, mat4x4 a, float k)
130 {
131 int i;
132 for(i=0; i<4; ++i)
133 vec4_scale(M[i], a[i], k);
134 }
mat4x4_scale_aniso(mat4x4 M,mat4x4 a,float x,float y,float z)135 static inline void mat4x4_scale_aniso(mat4x4 M, mat4x4 a, float x, float y, float z)
136 {
137 int i;
138 vec4_scale(M[0], a[0], x);
139 vec4_scale(M[1], a[1], y);
140 vec4_scale(M[2], a[2], z);
141 for(i = 0; i < 4; ++i) {
142 M[3][i] = a[3][i];
143 }
144 }
mat4x4_mul(mat4x4 M,mat4x4 a,mat4x4 b)145 static inline void mat4x4_mul(mat4x4 M, mat4x4 a, mat4x4 b)
146 {
147 mat4x4 temp;
148 int k, r, c;
149 for(c=0; c<4; ++c) for(r=0; r<4; ++r) {
150 temp[c][r] = 0.f;
151 for(k=0; k<4; ++k)
152 temp[c][r] += a[k][r] * b[c][k];
153 }
154 mat4x4_dup(M, temp);
155 }
mat4x4_mul_vec4(vec4 r,mat4x4 M,vec4 v)156 static inline void mat4x4_mul_vec4(vec4 r, mat4x4 M, vec4 v)
157 {
158 int i, j;
159 for(j=0; j<4; ++j) {
160 r[j] = 0.f;
161 for(i=0; i<4; ++i)
162 r[j] += M[i][j] * v[i];
163 }
164 }
mat4x4_translate(mat4x4 T,float x,float y,float z)165 static inline void mat4x4_translate(mat4x4 T, float x, float y, float z)
166 {
167 mat4x4_identity(T);
168 T[3][0] = x;
169 T[3][1] = y;
170 T[3][2] = z;
171 }
mat4x4_translate_in_place(mat4x4 M,float x,float y,float z)172 static inline void mat4x4_translate_in_place(mat4x4 M, float x, float y, float z)
173 {
174 vec4 t = {x, y, z, 0};
175 vec4 r;
176 int i;
177 for (i = 0; i < 4; ++i) {
178 mat4x4_row(r, M, i);
179 M[3][i] += vec4_mul_inner(r, t);
180 }
181 }
mat4x4_from_vec3_mul_outer(mat4x4 M,vec3 a,vec3 b)182 static inline void mat4x4_from_vec3_mul_outer(mat4x4 M, vec3 a, vec3 b)
183 {
184 int i, j;
185 for(i=0; i<4; ++i) for(j=0; j<4; ++j)
186 M[i][j] = i<3 && j<3 ? a[i] * b[j] : 0.f;
187 }
mat4x4_rotate(mat4x4 R,mat4x4 M,float x,float y,float z,float angle)188 static inline void mat4x4_rotate(mat4x4 R, mat4x4 M, float x, float y, float z, float angle)
189 {
190 float s = sinf(angle);
191 float c = cosf(angle);
192 vec3 u = {x, y, z};
193
194 if(vec3_len(u) > 1e-4) {
195 mat4x4 T, C, S;
196
197 vec3_norm(u, u);
198 mat4x4_from_vec3_mul_outer(T, u, u);
199
200 S[1][2] = u[0];
201 S[2][1] = -u[0];
202 S[2][0] = u[1];
203 S[0][2] = -u[1];
204 S[0][1] = u[2];
205 S[1][0] = -u[2];
206
207 mat4x4_scale(S, S, s);
208
209 mat4x4_identity(C);
210 mat4x4_sub(C, C, T);
211
212 mat4x4_scale(C, C, c);
213
214 mat4x4_add(T, T, C);
215 mat4x4_add(T, T, S);
216
217 T[3][3] = 1.;
218 mat4x4_mul(R, M, T);
219 } else {
220 mat4x4_dup(R, M);
221 }
222 }
mat4x4_rotate_X(mat4x4 Q,mat4x4 M,float angle)223 static inline void mat4x4_rotate_X(mat4x4 Q, mat4x4 M, float angle)
224 {
225 float s = sinf(angle);
226 float c = cosf(angle);
227 mat4x4 R = {
228 {1.f, 0.f, 0.f, 0.f},
229 {0.f, c, s, 0.f},
230 {0.f, -s, c, 0.f},
231 {0.f, 0.f, 0.f, 1.f}
232 };
233 mat4x4_mul(Q, M, R);
234 }
mat4x4_rotate_Y(mat4x4 Q,mat4x4 M,float angle)235 static inline void mat4x4_rotate_Y(mat4x4 Q, mat4x4 M, float angle)
236 {
237 float s = sinf(angle);
238 float c = cosf(angle);
239 mat4x4 R = {
240 { c, 0.f, s, 0.f},
241 { 0.f, 1.f, 0.f, 0.f},
242 { -s, 0.f, c, 0.f},
243 { 0.f, 0.f, 0.f, 1.f}
244 };
245 mat4x4_mul(Q, M, R);
246 }
mat4x4_rotate_Z(mat4x4 Q,mat4x4 M,float angle)247 static inline void mat4x4_rotate_Z(mat4x4 Q, mat4x4 M, float angle)
248 {
249 float s = sinf(angle);
250 float c = cosf(angle);
251 mat4x4 R = {
252 { c, s, 0.f, 0.f},
253 { -s, c, 0.f, 0.f},
254 { 0.f, 0.f, 1.f, 0.f},
255 { 0.f, 0.f, 0.f, 1.f}
256 };
257 mat4x4_mul(Q, M, R);
258 }
mat4x4_invert(mat4x4 T,mat4x4 M)259 static inline void mat4x4_invert(mat4x4 T, mat4x4 M)
260 {
261 float idet;
262 float s[6];
263 float c[6];
264 s[0] = M[0][0]*M[1][1] - M[1][0]*M[0][1];
265 s[1] = M[0][0]*M[1][2] - M[1][0]*M[0][2];
266 s[2] = M[0][0]*M[1][3] - M[1][0]*M[0][3];
267 s[3] = M[0][1]*M[1][2] - M[1][1]*M[0][2];
268 s[4] = M[0][1]*M[1][3] - M[1][1]*M[0][3];
269 s[5] = M[0][2]*M[1][3] - M[1][2]*M[0][3];
270
271 c[0] = M[2][0]*M[3][1] - M[3][0]*M[2][1];
272 c[1] = M[2][0]*M[3][2] - M[3][0]*M[2][2];
273 c[2] = M[2][0]*M[3][3] - M[3][0]*M[2][3];
274 c[3] = M[2][1]*M[3][2] - M[3][1]*M[2][2];
275 c[4] = M[2][1]*M[3][3] - M[3][1]*M[2][3];
276 c[5] = M[2][2]*M[3][3] - M[3][2]*M[2][3];
277
278 /* Assumes it is invertible */
279 idet = 1.0f/( s[0]*c[5]-s[1]*c[4]+s[2]*c[3]+s[3]*c[2]-s[4]*c[1]+s[5]*c[0] );
280
281 T[0][0] = ( M[1][1] * c[5] - M[1][2] * c[4] + M[1][3] * c[3]) * idet;
282 T[0][1] = (-M[0][1] * c[5] + M[0][2] * c[4] - M[0][3] * c[3]) * idet;
283 T[0][2] = ( M[3][1] * s[5] - M[3][2] * s[4] + M[3][3] * s[3]) * idet;
284 T[0][3] = (-M[2][1] * s[5] + M[2][2] * s[4] - M[2][3] * s[3]) * idet;
285
286 T[1][0] = (-M[1][0] * c[5] + M[1][2] * c[2] - M[1][3] * c[1]) * idet;
287 T[1][1] = ( M[0][0] * c[5] - M[0][2] * c[2] + M[0][3] * c[1]) * idet;
288 T[1][2] = (-M[3][0] * s[5] + M[3][2] * s[2] - M[3][3] * s[1]) * idet;
289 T[1][3] = ( M[2][0] * s[5] - M[2][2] * s[2] + M[2][3] * s[1]) * idet;
290
291 T[2][0] = ( M[1][0] * c[4] - M[1][1] * c[2] + M[1][3] * c[0]) * idet;
292 T[2][1] = (-M[0][0] * c[4] + M[0][1] * c[2] - M[0][3] * c[0]) * idet;
293 T[2][2] = ( M[3][0] * s[4] - M[3][1] * s[2] + M[3][3] * s[0]) * idet;
294 T[2][3] = (-M[2][0] * s[4] + M[2][1] * s[2] - M[2][3] * s[0]) * idet;
295
296 T[3][0] = (-M[1][0] * c[3] + M[1][1] * c[1] - M[1][2] * c[0]) * idet;
297 T[3][1] = ( M[0][0] * c[3] - M[0][1] * c[1] + M[0][2] * c[0]) * idet;
298 T[3][2] = (-M[3][0] * s[3] + M[3][1] * s[1] - M[3][2] * s[0]) * idet;
299 T[3][3] = ( M[2][0] * s[3] - M[2][1] * s[1] + M[2][2] * s[0]) * idet;
300 }
mat4x4_orthonormalize(mat4x4 R,mat4x4 M)301 static inline void mat4x4_orthonormalize(mat4x4 R, mat4x4 M)
302 {
303 float s = 1.;
304 vec3 h;
305
306 mat4x4_dup(R, M);
307 vec3_norm(R[2], R[2]);
308
309 s = vec3_mul_inner(R[1], R[2]);
310 vec3_scale(h, R[2], s);
311 vec3_sub(R[1], R[1], h);
312 vec3_norm(R[2], R[2]);
313
314 s = vec3_mul_inner(R[1], R[2]);
315 vec3_scale(h, R[2], s);
316 vec3_sub(R[1], R[1], h);
317 vec3_norm(R[1], R[1]);
318
319 s = vec3_mul_inner(R[0], R[1]);
320 vec3_scale(h, R[1], s);
321 vec3_sub(R[0], R[0], h);
322 vec3_norm(R[0], R[0]);
323 }
324
mat4x4_frustum(mat4x4 M,float l,float r,float b,float t,float n,float f)325 static inline void mat4x4_frustum(mat4x4 M, float l, float r, float b, float t, float n, float f)
326 {
327 M[0][0] = 2.f*n/(r-l);
328 M[0][1] = M[0][2] = M[0][3] = 0.f;
329
330 M[1][1] = 2.f*n/(t-b);
331 M[1][0] = M[1][2] = M[1][3] = 0.f;
332
333 M[2][0] = (r+l)/(r-l);
334 M[2][1] = (t+b)/(t-b);
335 M[2][2] = -(f+n)/(f-n);
336 M[2][3] = -1.f;
337
338 M[3][2] = -2.f*(f*n)/(f-n);
339 M[3][0] = M[3][1] = M[3][3] = 0.f;
340 }
mat4x4_ortho(mat4x4 M,float l,float r,float b,float t,float n,float f)341 static inline void mat4x4_ortho(mat4x4 M, float l, float r, float b, float t, float n, float f)
342 {
343 M[0][0] = 2.f/(r-l);
344 M[0][1] = M[0][2] = M[0][3] = 0.f;
345
346 M[1][1] = 2.f/(t-b);
347 M[1][0] = M[1][2] = M[1][3] = 0.f;
348
349 M[2][2] = -2.f/(f-n);
350 M[2][0] = M[2][1] = M[2][3] = 0.f;
351
352 M[3][0] = -(r+l)/(r-l);
353 M[3][1] = -(t+b)/(t-b);
354 M[3][2] = -(f+n)/(f-n);
355 M[3][3] = 1.f;
356 }
mat4x4_perspective(mat4x4 m,float y_fov,float aspect,float n,float f)357 static inline void mat4x4_perspective(mat4x4 m, float y_fov, float aspect, float n, float f)
358 {
359 /* NOTE: Degrees are an unhandy unit to work with.
360 * linmath.h uses radians for everything! */
361 float const a = 1.f / (float) tan(y_fov / 2.f);
362
363 m[0][0] = a / aspect;
364 m[0][1] = 0.f;
365 m[0][2] = 0.f;
366 m[0][3] = 0.f;
367
368 m[1][0] = 0.f;
369 m[1][1] = a;
370 m[1][2] = 0.f;
371 m[1][3] = 0.f;
372
373 m[2][0] = 0.f;
374 m[2][1] = 0.f;
375 m[2][2] = -((f + n) / (f - n));
376 m[2][3] = -1.f;
377
378 m[3][0] = 0.f;
379 m[3][1] = 0.f;
380 m[3][2] = -((2.f * f * n) / (f - n));
381 m[3][3] = 0.f;
382 }
mat4x4_look_at(mat4x4 m,vec3 eye,vec3 center,vec3 up)383 static inline void mat4x4_look_at(mat4x4 m, vec3 eye, vec3 center, vec3 up)
384 {
385 /* Adapted from Android's OpenGL Matrix.java. */
386 /* See the OpenGL GLUT documentation for gluLookAt for a description */
387 /* of the algorithm. We implement it in a straightforward way: */
388
389 /* TODO: The negation of of can be spared by swapping the order of
390 * operands in the following cross products in the right way. */
391 vec3 f;
392 vec3 s;
393 vec3 t;
394
395 vec3_sub(f, center, eye);
396 vec3_norm(f, f);
397
398 vec3_mul_cross(s, f, up);
399 vec3_norm(s, s);
400
401 vec3_mul_cross(t, s, f);
402
403 m[0][0] = s[0];
404 m[0][1] = t[0];
405 m[0][2] = -f[0];
406 m[0][3] = 0.f;
407
408 m[1][0] = s[1];
409 m[1][1] = t[1];
410 m[1][2] = -f[1];
411 m[1][3] = 0.f;
412
413 m[2][0] = s[2];
414 m[2][1] = t[2];
415 m[2][2] = -f[2];
416 m[2][3] = 0.f;
417
418 m[3][0] = 0.f;
419 m[3][1] = 0.f;
420 m[3][2] = 0.f;
421 m[3][3] = 1.f;
422
423 mat4x4_translate_in_place(m, -eye[0], -eye[1], -eye[2]);
424 }
425
426 typedef float quat[4];
quat_identity(quat q)427 static inline void quat_identity(quat q)
428 {
429 q[0] = q[1] = q[2] = 0.f;
430 q[3] = 1.f;
431 }
quat_add(quat r,quat a,quat b)432 static inline void quat_add(quat r, quat a, quat b)
433 {
434 int i;
435 for(i=0; i<4; ++i)
436 r[i] = a[i] + b[i];
437 }
quat_sub(quat r,quat a,quat b)438 static inline void quat_sub(quat r, quat a, quat b)
439 {
440 int i;
441 for(i=0; i<4; ++i)
442 r[i] = a[i] - b[i];
443 }
quat_mul(quat r,quat p,quat q)444 static inline void quat_mul(quat r, quat p, quat q)
445 {
446 vec3 w;
447 vec3_mul_cross(r, p, q);
448 vec3_scale(w, p, q[3]);
449 vec3_add(r, r, w);
450 vec3_scale(w, q, p[3]);
451 vec3_add(r, r, w);
452 r[3] = p[3]*q[3] - vec3_mul_inner(p, q);
453 }
quat_scale(quat r,quat v,float s)454 static inline void quat_scale(quat r, quat v, float s)
455 {
456 int i;
457 for(i=0; i<4; ++i)
458 r[i] = v[i] * s;
459 }
quat_inner_product(quat a,quat b)460 static inline float quat_inner_product(quat a, quat b)
461 {
462 float p = 0.f;
463 int i;
464 for(i=0; i<4; ++i)
465 p += b[i]*a[i];
466 return p;
467 }
quat_conj(quat r,quat q)468 static inline void quat_conj(quat r, quat q)
469 {
470 int i;
471 for(i=0; i<3; ++i)
472 r[i] = -q[i];
473 r[3] = q[3];
474 }
quat_rotate(quat r,float angle,vec3 axis)475 static inline void quat_rotate(quat r, float angle, vec3 axis) {
476 int i;
477 vec3 v;
478 vec3_scale(v, axis, sinf(angle / 2));
479 for(i=0; i<3; ++i)
480 r[i] = v[i];
481 r[3] = cosf(angle / 2);
482 }
483 #define quat_norm vec4_norm
quat_mul_vec3(vec3 r,quat q,vec3 v)484 static inline void quat_mul_vec3(vec3 r, quat q, vec3 v)
485 {
486 /*
487 * Method by Fabian 'ryg' Giessen (of Farbrausch)
488 t = 2 * cross(q.xyz, v)
489 v' = v + q.w * t + cross(q.xyz, t)
490 */
491 vec3 t = {q[0], q[1], q[2]};
492 vec3 u = {q[0], q[1], q[2]};
493
494 vec3_mul_cross(t, t, v);
495 vec3_scale(t, t, 2);
496
497 vec3_mul_cross(u, u, t);
498 vec3_scale(t, t, q[3]);
499
500 vec3_add(r, v, t);
501 vec3_add(r, r, u);
502 }
mat4x4_from_quat(mat4x4 M,quat q)503 static inline void mat4x4_from_quat(mat4x4 M, quat q)
504 {
505 float a = q[3];
506 float b = q[0];
507 float c = q[1];
508 float d = q[2];
509 float a2 = a*a;
510 float b2 = b*b;
511 float c2 = c*c;
512 float d2 = d*d;
513
514 M[0][0] = a2 + b2 - c2 - d2;
515 M[0][1] = 2.f*(b*c + a*d);
516 M[0][2] = 2.f*(b*d - a*c);
517 M[0][3] = 0.f;
518
519 M[1][0] = 2*(b*c - a*d);
520 M[1][1] = a2 - b2 + c2 - d2;
521 M[1][2] = 2.f*(c*d + a*b);
522 M[1][3] = 0.f;
523
524 M[2][0] = 2.f*(b*d + a*c);
525 M[2][1] = 2.f*(c*d - a*b);
526 M[2][2] = a2 - b2 - c2 + d2;
527 M[2][3] = 0.f;
528
529 M[3][0] = M[3][1] = M[3][2] = 0.f;
530 M[3][3] = 1.f;
531 }
532
mat4x4o_mul_quat(mat4x4 R,mat4x4 M,quat q)533 static inline void mat4x4o_mul_quat(mat4x4 R, mat4x4 M, quat q)
534 {
535 /* XXX: The way this is written only works for othogonal matrices. */
536 /* TODO: Take care of non-orthogonal case. */
537 quat_mul_vec3(R[0], q, M[0]);
538 quat_mul_vec3(R[1], q, M[1]);
539 quat_mul_vec3(R[2], q, M[2]);
540
541 R[3][0] = R[3][1] = R[3][2] = 0.f;
542 R[3][3] = 1.f;
543 }
quat_from_mat4x4(quat q,mat4x4 M)544 static inline void quat_from_mat4x4(quat q, mat4x4 M)
545 {
546 float r=0.f;
547 int i;
548
549 int perm[] = { 0, 1, 2, 0, 1 };
550 int *p = perm;
551
552 for(i = 0; i<3; i++) {
553 float m = M[i][i];
554 if( m < r )
555 continue;
556 m = r;
557 p = &perm[i];
558 }
559
560 r = (float) sqrt(1.f + M[p[0]][p[0]] - M[p[1]][p[1]] - M[p[2]][p[2]] );
561
562 if(r < 1e-6) {
563 q[0] = 1.f;
564 q[1] = q[2] = q[3] = 0.f;
565 return;
566 }
567
568 q[0] = r/2.f;
569 q[1] = (M[p[0]][p[1]] - M[p[1]][p[0]])/(2.f*r);
570 q[2] = (M[p[2]][p[0]] - M[p[0]][p[2]])/(2.f*r);
571 q[3] = (M[p[2]][p[1]] - M[p[1]][p[2]])/(2.f*r);
572 }
573
574 #endif
575