projects / fishladder.git / blob
? search:


a0c01077048b0a5cd599f206e428b474ca3af1d9
[fishladder.git] / vg / vg_m.h
1 // Copyright (C) 2021 Harry Godden (hgn) - All Rights Reserved
2
3 // Util
4 // ==================================================================================================================
5
6 #define VG_PIf 3.14159265358979323846264338327950288f
7 #define VG_TAUf 6.28318530717958647692528676655900576f
8
9 // Simple min/max replacements
10 static inline float vg_minf( float a, float b )
11 {
12 return a < b? a: b;
13 }
14
15 static inline float vg_maxf( float a, float b )
16 {
17 return a > b? a: b;
18 }
19
20 static inline float vg_clampf( float a, float min, float max )
21 {
22 return vg_minf( max, vg_maxf( a, min ) );
23 }
24
25 #define VG_MIN( A, B ) ((A)<(B)?(A):(B))
26 #define VG_MAX( A, B ) ((A)>(B)?(A):(B))
27
28 // Hopefully deprecate this!!
29 static inline int vg_min( int a, int b )
30 {
31 return a < b? a: b;
32 }
33
34 static inline int vg_max( int a, int b )
35 {
36 return a > b? a: b;
37 }
38
39 // Convert degrees to radians
40 static inline float vg_rad( float deg )
41 {
42 return deg * VG_PIf / 180.0f;
43 }
44
45 // Vector 2
46 // ==================================================================================================================
47
48 static inline void v2_copy( v2f a, v2f b )
49 {
50 b[0] = a[0]; b[1] = a[1];
51 }
52
53 static inline void v2i_copy( v2i a, v2i b )
54 {
55 b[0] = a[0]; b[1] = a[1];
56 }
57
58 static inline void v2i_add( v2i a, v2i b, v2i d )
59 {
60 d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
61 }
62
63 static inline void v2i_sub( v2i a, v2i b, v2i d )
64 {
65 d[0] = a[0]-b[0]; d[1] = a[1]-b[1];
66 }
67
68 static inline void v2_minv( v2f a, v2f b, v2f dest )
69 {
70 dest[0] = vg_minf(a[0], b[0]);
71 dest[1] = vg_minf(a[1], b[1]);
72 }
73
74 static inline void v2_maxv( v2f a, v2f b, v2f dest )
75 {
76 dest[0] = vg_maxf(a[0], b[0]);
77 dest[1] = vg_maxf(a[1], b[1]);
78 }
79
80 static inline void v2_sub( v2f a, v2f b, v2f d )
81 {
82 d[0] = a[0]-b[0]; d[1] = a[1]-b[1];
83 }
84
85 static inline float v2_cross( v2f a, v2f b )
86 {
87 return a[0] * b[1] - a[1] * b[0];
88 }
89
90 static inline void v2_add( v2f a, v2f b, v2f d )
91 {
92 d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
93 }
94
95 static inline void v2_muls( v2f a, float s, v2f d )
96 {
97 d[0] = a[0]*s; d[1] = a[1]*s;
98 }
99
100 static inline void v2_divs( v2f a, float s, v2f d )
101 {
102 d[0] = a[0]/s; d[1] = a[1]/s;
103 }
104
105
106 static inline void v2_mul( v2f a, v2f b, v2f d )
107 {
108 d[0] = a[0]*b[0]; d[1] = a[1]*b[1];
109 }
110
111 static inline void v2_div( v2f a, v2f b, v2f d )
112 {
113 d[0] = a[0]/b[0]; d[1] = a[1]/b[1];
114 }
115
116 static inline void v2_muladds( v2f a, v2f b, float s, v2f d )
117 {
118 d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s;
119 }
120
121 static inline float v2_length2( v2f a )
122 {
123 return a[0]*a[0] + a[1]*a[1];
124 }
125
126 static inline float v2_length( v2f a )
127 {
128 return sqrtf( v2_length2( a ) );
129 }
130
131 static inline float v2_dist2( v2f a, v2f b )
132 {
133 v2f delta;
134 v2_sub( a, b, delta );
135 return v2_length2( delta );
136 }
137
138 static inline float v2_dist( v2f a, v2f b )
139 {
140 return sqrtf( v2_dist2( a, b ) );
141 }
142
143 // Vector 3
144 // ==================================================================================================================
145
146 static inline void v3_zero( v3f a )
147 {
148 a[0] = 0.f; a[1] = 0.f; a[2] = 0.f;
149 }
150
151 static inline void v3_copy( v3f a, v3f b )
152 {
153 b[0] = a[0]; b[1] = a[1]; b[2] = a[2];
154 }
155
156 static inline void v3_add( v3f a, v3f b, v3f d )
157 {
158 d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2];
159 }
160
161 static inline void v3_sub( v3f a, v3f b, v3f d )
162 {
163 d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2];
164 }
165
166 static inline void v3_mul( v3f a, v3f b, v3f d )
167 {
168 d[0] = a[0]*b[0]; d[1] = a[1]*b[1]; d[2] = a[2]*b[2];
169 }
170
171 static inline void v3_div( v3f a, v3f b, v3f d )
172 {
173 d[0] = a[0]/b[0]; d[1] = a[1]/b[1]; d[2] = a[2]/b[2];
174 }
175
176 static inline void v3_muls( v3f a, float s, v3f d )
177 {
178 d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s;
179 }
180
181 static inline void v3_divs( v3f a, float s, v3f d )
182 {
183 d[0] = a[0]/s; d[1] = a[1]/s; d[2] = a[2]/s;
184 }
185
186 static inline void v3_muladds( v3f a, v3f b, float s, v3f d )
187 {
188 d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; d[2] = a[2]+b[2]*s;
189 }
190
191 static inline float v3_dot( v3f a, v3f b )
192 {
193 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
194 }
195
196 static inline void v3_cross( v3f a, v3f b, v3f d )
197 {
198 d[0] = a[1] * b[2] - a[2] * b[1];
199 d[1] = a[2] * b[0] - a[0] * b[2];
200 d[2] = a[0] * b[1] - a[1] * b[0];
201 }
202
203 static inline float v3_length2( v3f a )
204 {
205 return v3_dot( a, a );
206 }
207
208 static inline float v3_length( v3f a )
209 {
210 return sqrtf( v3_length2( a ) );
211 }
212
213 static inline float v3_dist2( v3f a, v3f b )
214 {
215 v3f delta;
216 v3_sub( a, b, delta );
217 return v3_length2( delta );
218 }
219
220 static inline float v3_dist( v3f a, v3f b )
221 {
222 return sqrtf( v3_dist2( a, b ) );
223 }
224
225 static inline void v3_normalize( v3f a )
226 {
227 v3_muls( a, 1.f / v3_length( a ), a );
228 }
229
230 static inline float csr_lerpf( float a, float b, float t )
231 {
232 return a + t*(b-a);
233 }
234
235 static inline void v3_lerp( v3f a, v3f b, float t, v3f d )
236 {
237 d[0] = a[0] + t*(b[0]-a[0]);
238 d[1] = a[1] + t*(b[1]-a[1]);
239 d[2] = a[2] + t*(b[2]-a[2]);
240 }
241
242 static inline void v3_minv( v3f a, v3f b, v3f dest )
243 {
244 dest[0] = vg_minf(a[0], b[0]);
245 dest[1] = vg_minf(a[1], b[1]);
246 dest[2] = vg_minf(a[2], b[2]);
247 }
248
249 static inline void v3_maxv( v3f a, v3f b, v3f dest )
250 {
251 dest[0] = vg_maxf(a[0], b[0]);
252 dest[1] = vg_maxf(a[1], b[1]);
253 dest[2] = vg_maxf(a[2], b[2]);
254 }
255
256 static inline float v3_minf( v3f a )
257 {
258 return vg_minf( vg_minf( a[0], a[1] ), a[2] );
259 }
260
261 static inline float v3_maxf( v3f a )
262 {
263 return vg_maxf( vg_maxf( a[0], a[1] ), a[2] );
264 }
265
266 static inline void v3_fill( v3f a, float v )
267 {
268 a[0] = v;
269 a[1] = v;
270 a[2] = v;
271 }
272
273 // Vector 4
274 // ==================================================================================================================
275
276 static inline void v4_copy( v4f a, v4f b )
277 {
278 b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3];
279 }
280
281 static inline void v4_zero( v4f a )
282 {
283 a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; a[3] = 0.f;
284 }
285
286 // Matrix 2x2
287 // ===========================================================================================================
288
289 #define M2X2_INDENTIY {{1.0f, 0.0f, }, \
290 { 0.0f, 1.0f, }}
291
292 #define M2X2_ZERO {{0.0f, 0.0f, }, \
293 { 0.0f, 0.0f, }}
294
295 static inline void m2x2_copy( m2x2f a, m2x2f b )
296 {
297 v2_copy( a[0], b[0] );
298 v2_copy( a[1], b[1] );
299 }
300
301 static inline void m2x2_identity( m2x2f a )
302 {
303 m2x2f id = M2X2_INDENTIY;
304 m2x2_copy( id, a );
305 }
306
307 static inline void m2x2_create_rotation( m2x2f a, float theta )
308 {
309 float s, c;
310
311 s = sinf( theta );
312 c = cosf( theta );
313
314 a[0][0] = c;
315 a[0][1] = -s;
316 a[1][0] = s;
317 a[1][1] = c;
318 }
319
320 // Matrix 3x3
321 //======================================================================================================
322
323 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
324 { 0.0f, 1.0f, 0.0f, },\
325 { 0.0f, 0.0f, 1.0f, }}
326
327 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
328 { 0.0f, 0.0f, 0.0f, },\
329 { 0.0f, 0.0f, 0.0f, }}
330
331
332 static inline void m3x3_copy( m3x3f a, m3x3f b )
333 {
334 v3_copy( a[0], b[0] );
335 v3_copy( a[1], b[1] );
336 v3_copy( a[2], b[2] );
337 }
338
339 static inline void m3x3_identity( m3x3f a )
340 {
341 m3x3f id = M3X3_IDENTITY;
342 m3x3_copy( id, a );
343 }
344
345 static inline void m3x3_zero( m3x3f a )
346 {
347 m3x3f z = M3X3_ZERO;
348 m3x3_copy( z, a );
349 }
350
351 static inline void m3x3_inv( m3x3f src, m3x3f dest )
352 {
353 float a = src[0][0], b = src[0][1], c = src[0][2],
354 d = src[1][0], e = src[1][1], f = src[1][2],
355 g = src[2][0], h = src[2][1], i = src[2][2];
356
357 float det = 1.f /
358 (+a*(e*i-h*f)
359 -b*(d*i-f*g)
360 +c*(d*h-e*g));
361
362 dest[0][0] = (e*i-h*f)*det;
363 dest[0][1] = -(b*i-c*h)*det;
364 dest[0][2] = (b*f-c*e)*det;
365 dest[1][0] = -(d*i-f*g)*det;
366 dest[1][1] = (a*i-c*g)*det;
367 dest[1][2] = -(a*f-d*c)*det;
368 dest[2][0] = (d*h-g*e)*det;
369 dest[2][1] = -(a*h-g*b)*det;
370 dest[2][2] = (a*e-d*b)*det;
371 }
372
373 static inline void m3x3_transpose( m3x3f src, m3x3f dest )
374 {
375 float a = src[0][0], b = src[0][1], c = src[0][2],
376 d = src[1][0], e = src[1][1], f = src[1][2],
377 g = src[2][0], h = src[2][1], i = src[2][2];
378
379 dest[0][0] = a;
380 dest[0][1] = d;
381 dest[0][2] = g;
382 dest[1][0] = b;
383 dest[1][1] = e;
384 dest[1][2] = h;
385 dest[2][0] = c;
386 dest[2][1] = f;
387 dest[2][2] = i;
388 }
389
390 static inline void m3x3_mul( m3x3f a, m3x3f b, m3x3f d )
391 {
392 float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2],
393 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2],
394 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2],
395
396 b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
397 b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
398 b20 = b[2][0], b21 = b[2][1], b22 = b[2][2];
399
400 d[0][0] = a00*b00 + a10*b01 + a20*b02;
401 d[0][1] = a01*b00 + a11*b01 + a21*b02;
402 d[0][2] = a02*b00 + a12*b01 + a22*b02;
403 d[1][0] = a00*b10 + a10*b11 + a20*b12;
404 d[1][1] = a01*b10 + a11*b11 + a21*b12;
405 d[1][2] = a02*b10 + a12*b11 + a22*b12;
406 d[2][0] = a00*b20 + a10*b21 + a20*b22;
407 d[2][1] = a01*b20 + a11*b21 + a21*b22;
408 d[2][2] = a02*b20 + a12*b21 + a22*b22;
409 }
410
411 static inline void m3x3_mulv( m3x3f m, v3f v, v3f d )
412 {
413 v3f res;
414
415 res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
416 res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
417 res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
418
419 v3_copy( res, d );
420 }
421
422 static inline void m3x3_projection( m3x3f dst, float const left, float const right, float const bottom, float const top )
423 {
424 float rl, tb;
425
426 m3x3_zero( dst );
427
428 rl = 1.0f / (right - left);
429 tb = 1.0f / (top - bottom);
430
431 dst[0][0] = 2.0f * rl;
432 dst[1][1] = 2.0f * tb;
433 dst[2][2] = 1.0f;
434 }
435
436 static inline void m3x3_translate( m3x3f m, v3f v )
437 {
438 m[2][0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0];
439 m[2][1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1];
440 m[2][2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2];
441 }
442
443 static inline void m3x3_scale( m3x3f m, v3f v )
444 {
445 m[0][0] = m[0][0] * v[0];
446 m[0][1] = m[0][1] * v[0];
447 m[0][2] = m[0][2] * v[0];
448
449 m[1][0] = m[1][0] * v[1];
450 m[1][1] = m[1][1] * v[1];
451 m[1][2] = m[1][2] * v[1];
452 }
453
454 static inline void m3x3_rotate( m3x3f m, float angle )
455 {
456 float m00 = m[0][0], m10 = m[1][0],
457 m01 = m[0][1], m11 = m[1][1],
458 m02 = m[0][2], m12 = m[1][2];
459 float c, s;
460
461 s = sinf( angle );
462 c = cosf( angle );
463
464 m[0][0] = m00 * c + m10 * s;
465 m[0][1] = m01 * c + m11 * s;
466 m[0][2] = m02 * c + m12 * s;
467
468 m[1][0] = m00 * -s + m10 * c;
469 m[1][1] = m01 * -s + m11 * c;
470 m[1][2] = m02 * -s + m12 * c;
471 }
472
473 // Matrix 4x3
474 // ==================================================================================================================
475
476 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
477 { 0.0f, 1.0f, 0.0f, },\
478 { 0.0f, 0.0f, 1.0f, },\
479 { 0.0f, 0.0f, 0.0f }}
480
481 static inline void m4x3_to_3x3( m4x3f a, m3x3f b )
482 {
483 v3_copy( a[0], b[0] );
484 v3_copy( a[1], b[1] );
485 v3_copy( a[2], b[2] );
486 }
487
488 static inline void m4x3_copy( m4x3f a, m4x3f b )
489 {
490 v3_copy( a[0], b[0] );
491 v3_copy( a[1], b[1] );
492 v3_copy( a[2], b[2] );
493 v3_copy( a[3], b[3] );
494 }
495
496 static inline void m4x3_identity( m4x3f a )
497 {
498 m4x3f id = M4X3_IDENTITY;
499 m4x3_copy( id, a );
500 }
501
502 static inline void m4x3_mul( m4x3f a, m4x3f b, m4x3f d )
503 {
504 float
505 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2],
506 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2],
507 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2],
508 a30 = a[3][0], a31 = a[3][1], a32 = a[3][2],
509 b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
510 b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
511 b20 = b[2][0], b21 = b[2][1], b22 = b[2][2],
512 b30 = b[3][0], b31 = b[3][1], b32 = b[3][2];
513
514 d[0][0] = a00*b00 + a10*b01 + a20*b02;
515 d[0][1] = a01*b00 + a11*b01 + a21*b02;
516 d[0][2] = a02*b00 + a12*b01 + a22*b02;
517 d[1][0] = a00*b10 + a10*b11 + a20*b12;
518 d[1][1] = a01*b10 + a11*b11 + a21*b12;
519 d[1][2] = a02*b10 + a12*b11 + a22*b12;
520 d[2][0] = a00*b20 + a10*b21 + a20*b22;
521 d[2][1] = a01*b20 + a11*b21 + a21*b22;
522 d[2][2] = a02*b20 + a12*b21 + a22*b22;
523 d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30;
524 d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31;
525 d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32;
526 }
527
528 static inline void m4x3_mulv( m4x3f m, v3f v, v3f d )
529 {
530 v3f res;
531
532 res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0];
533 res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1];
534 res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2];
535
536 v3_copy( res, d );
537 }
538
539 // Affine transforms
540 // ====================================================================================================================
541
542 static inline void m4x3_translate( m4x3f m, v3f v )
543 {
544 v3_muladds( m[3], m[0], v[0], m[3] );
545 v3_muladds( m[3], m[1], v[1], m[3] );
546 v3_muladds( m[3], m[2], v[2], m[3] );
547 }
548
549 static inline void m4x3_scale( m4x3f m, float s )
550 {
551 v3_muls( m[0], s, m[0] );
552 v3_muls( m[1], s, m[1] );
553 v3_muls( m[2], s, m[2] );
554 }
555
556 static inline void m4x3_rotate_x( m4x3f m, float angle )
557 {
558 m4x3f t = M4X3_IDENTITY;
559 float c, s;
560
561 c = cosf( angle );
562 s = sinf( angle );
563
564 t[1][1] = c;
565 t[1][2] = s;
566 t[2][1] = -s;
567 t[2][2] = c;
568
569 m4x3_mul( m, t, m );
570 }
571
572 static inline void m4x3_rotate_y( m4x3f m, float angle )
573 {
574 m4x3f t = M4X3_IDENTITY;
575 float c, s;
576
577 c = cosf( angle );
578 s = sinf( angle );
579
580 t[0][0] = c;
581 t[0][2] = -s;
582 t[2][0] = s;
583 t[2][2] = c;
584
585 m4x3_mul( m, t, m );
586 }
587
588 static inline void m4x3_rotate_z( m4x3f m, float angle )
589 {
590 m4x3f t = M4X3_IDENTITY;
591 float c, s;
592
593 c = cosf( angle );
594 s = sinf( angle );
595
596 t[0][0] = c;
597 t[0][1] = s;
598 t[1][0] = -s;
599 t[1][1] = c;
600
601 m4x3_mul( m, t, m );
602 }
603
604 // Warning: These functions are unoptimized..
605 static inline void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point )
606 {
607 v3f v;
608 m4x3_mulv( m, point, v );
609
610 v3_minv( box[0], v, box[0] );
611 v3_maxv( box[1], v, box[1] );
612 }
613
614 static inline void box_concat( boxf a, boxf b )
615 {
616 v3_minv( a[0], b[0], a[0] );
617 v3_maxv( a[1], b[1], a[1] );
618 }
619
620 static inline void box_copy( boxf a, boxf b )
621 {
622 v3_copy( a[0], b[0] );
623 v3_copy( a[1], b[1] );
624 }
625
626 static inline void m4x3_transform_aabb( m4x3f m, boxf box )
627 {
628 v3f a; v3f b;
629
630 v3_copy( box[0], a );
631 v3_copy( box[1], b );
632 v3_fill( box[0], INFINITY );
633 v3_fill( box[1], -INFINITY );
634
635 m4x3_expand_aabb_point( m, box, a );
636 m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], a[2] } );
637 m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], a[2] } );
638 m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], a[2] } );
639 m4x3_expand_aabb_point( m, box, b );
640 m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], b[2] } );
641 m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], b[2] } );
642 m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], b[2] } );
643 }
644
645 // Planes (double precision)
646 // ==================================================================================================================
647
648 static inline void tri_to_plane( double a[3], double b[3], double c[3], double p[4] )
649 {
650 double edge0[3];
651 double edge1[3];
652 double l;
653
654 edge0[0] = b[0] - a[0];
655 edge0[1] = b[1] - a[1];
656 edge0[2] = b[2] - a[2];
657
658 edge1[0] = c[0] - a[0];
659 edge1[1] = c[1] - a[1];
660 edge1[2] = c[2] - a[2];
661
662 p[0] = edge0[1] * edge1[2] - edge0[2] * edge1[1];
663 p[1] = edge0[2] * edge1[0] - edge0[0] * edge1[2];
664 p[2] = edge0[0] * edge1[1] - edge0[1] * edge1[0];
665
666 l = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
667 p[3] = (p[0] * a[0] + p[1] * a[1] + p[2] * a[2]) / l;
668
669 p[0] = p[0] / l;
670 p[1] = p[1] / l;
671 p[2] = p[2] / l;
672 }
673
674 static inline int plane_intersect( double a[4], double b[4], double c[4], double p[4] )
675 {
676 double const epsilon = 1e-8f;
677
678 double x[3];
679 double d;
680
681 x[0] = a[1] * b[2] - a[2] * b[1];
682 x[1] = a[2] * b[0] - a[0] * b[2];
683 x[2] = a[0] * b[1] - a[1] * b[0];
684
685 d = x[0] * c[0] + x[1] * c[1] + x[2] * c[2];
686
687 if( d < epsilon && d > -epsilon ) return 0;
688
689 p[0] = (b[1] * c[2] - b[2] * c[1]) * -a[3];
690 p[1] = (b[2] * c[0] - b[0] * c[2]) * -a[3];
691 p[2] = (b[0] * c[1] - b[1] * c[0]) * -a[3];
692
693 p[0] += (c[1] * a[2] - c[2] * a[1]) * -b[3];
694 p[1] += (c[2] * a[0] - c[0] * a[2]) * -b[3];
695 p[2] += (c[0] * a[1] - c[1] * a[0]) * -b[3];
696
697 p[0] += (a[1] * b[2] - a[2] * b[1]) * -c[3];
698 p[1] += (a[2] * b[0] - a[0] * b[2]) * -c[3];
699 p[2] += (a[0] * b[1] - a[1] * b[0]) * -c[3];
700
701 p[0] = -p[0] / d;
702 p[1] = -p[1] / d;
703 p[2] = -p[2] / d;
704
705 return 1;
706 }
707
708 static inline double plane_polarity( double p[4], double a[3] )
709 {
710 return
711 (a[0] * p[0] + a[1] * p[1] + a[2] * p[2])
712 -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2])
713 ;
714 }
Nothing to see here...