1 /*
2 PLIB - A Suite of Portable Game Libraries
3 Copyright (C) 1998,2002 Steve Baker
4
5 This library is free software; you can redistribute it and/or
6 modify it under the terms of the GNU Library General Public
7 License as published by the Free Software Foundation; either
8 version 2 of the License, or (at your option) any later version.
9
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 Library General Public License for more details.
14
15 You should have received a copy of the GNU Library General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18
19 For further information visit http://plib.sourceforge.net
20
21 $Id: sg.cxx 1944 2004-08-05 01:07:09Z puggles $
22 */
23
24
25 #include "sg.h"
26
27 sgVec3 _sgGravity = { 0.0f, 0.0f, -9.8f } ;
28
sgVectorProductVec3(sgVec3 dst,const sgVec3 a,const sgVec3 b)29 void sgVectorProductVec3 ( sgVec3 dst, const sgVec3 a, const sgVec3 b )
30 {
31 dst[0] = a[1] * b[2] - a[2] * b[1] ;
32 dst[1] = a[2] * b[0] - a[0] * b[2] ;
33 dst[2] = a[0] * b[1] - a[1] * b[0] ;
34 }
35
_sgClampToUnity(const SGfloat x)36 inline SGfloat _sgClampToUnity ( const SGfloat x )
37 {
38 if ( x > SG_ONE ) return SG_ONE ;
39 if ( x < -SG_ONE ) return -SG_ONE ;
40 return x ;
41 }
42
sgCompare3DSqdDist(const sgVec3 v1,const sgVec3 v2,const SGfloat sqd_dist)43 int sgCompare3DSqdDist( const sgVec3 v1, const sgVec3 v2, const SGfloat sqd_dist )
44 {
45 sgVec3 tmp ;
46
47 sgSubVec3 ( tmp, v2, v1 ) ;
48
49 SGfloat sqdist = tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2] ;
50
51 if ( sqdist > sqd_dist ) return 1 ;
52 if ( sqdist < sqd_dist ) return -1 ;
53 return 0 ;
54 }
55
sgMakeRotMat4(sgMat4 mat,const SGfloat angle,const sgVec3 axis)56 void sgMakeRotMat4( sgMat4 mat, const SGfloat angle, const sgVec3 axis )
57 {
58 sgVec3 ax ;
59 sgNormalizeVec3 ( ax, axis ) ;
60
61 SGfloat temp_angle = angle * SG_DEGREES_TO_RADIANS ;
62 SGfloat s = (SGfloat) sin ( temp_angle ) ;
63 SGfloat c = (SGfloat) cos ( temp_angle ) ;
64 SGfloat t = SG_ONE - c ;
65
66 mat[0][0] = t * ax[0] * ax[0] + c ;
67 mat[0][1] = t * ax[0] * ax[1] + s * ax[2] ;
68 mat[0][2] = t * ax[0] * ax[2] - s * ax[1] ;
69 mat[0][3] = SG_ZERO ;
70
71 mat[1][0] = t * ax[1] * ax[0] - s * ax[2] ;
72 mat[1][1] = t * ax[1] * ax[1] + c ;
73 mat[1][2] = t * ax[1] * ax[2] + s * ax[0] ;
74 mat[1][3] = SG_ZERO ;
75
76 mat[2][0] = t * ax[2] * ax[0] + s * ax[1] ;
77 mat[2][1] = t * ax[2] * ax[1] - s * ax[0] ;
78 mat[2][2] = t * ax[2] * ax[2] + c ;
79 mat[2][3] = SG_ZERO ;
80
81 mat[3][0] = SG_ZERO ;
82 mat[3][1] = SG_ZERO ;
83 mat[3][2] = SG_ZERO ;
84 mat[3][3] = SG_ONE ;
85 }
86
87
88
89
sgMakePickMatrix(sgMat4 mat,sgFloat x,sgFloat y,sgFloat width,sgFloat height,sgVec4 viewport)90 void sgMakePickMatrix( sgMat4 mat, sgFloat x, sgFloat y,
91 sgFloat width, sgFloat height, sgVec4 viewport )
92 {
93 sgFloat sx = viewport[2] / width ;
94 sgFloat sy = viewport[3] / height ;
95 sgFloat tx = ( viewport[2] + SG_TWO * (viewport[0] - x) ) / width ;
96 sgFloat ty = ( viewport[3] + SG_TWO * (viewport[1] - y) ) / height ;
97
98 mat[0][0] = sx ;
99 mat[0][1] = SG_ZERO ;
100 mat[0][2] = SG_ZERO ;
101 mat[0][3] = SG_ZERO ;
102
103 mat[1][0] = SG_ZERO ;
104 mat[1][1] = sy ;
105 mat[1][2] = SG_ZERO ;
106 mat[1][3] = SG_ZERO ;
107
108 mat[2][0] = SG_ZERO ;
109 mat[2][1] = SG_ZERO ;
110 mat[2][2] = SG_ONE ;
111 mat[2][3] = SG_ZERO ;
112
113 mat[3][0] = tx ;
114 mat[3][1] = ty ;
115 mat[3][2] = SG_ZERO ;
116 mat[3][3] = SG_ONE ;
117 }
118
119
120
sgMakeLookAtMat4(sgMat4 dst,const sgVec3 eye,const sgVec3 center,const sgVec3 up)121 void sgMakeLookAtMat4 ( sgMat4 dst, const sgVec3 eye,
122 const sgVec3 center,
123 const sgVec3 up )
124 {
125 // Caveats:
126 // 1) In order to compute the line of sight, the eye point must not be equal
127 // to the center point.
128 // 2) The up vector must not be parallel to the line of sight from the eye
129 // to the center point.
130
131 /* Compute the direction vectors */
132 sgVec3 x,y,z;
133
134 /* Y vector = center - eye */
135 sgSubVec3 ( y, center, eye ) ;
136
137 /* Z vector = up */
138 sgCopyVec3 ( z, up ) ;
139
140 /* X vector = Y cross Z */
141 sgVectorProductVec3 ( x, y, z ) ;
142
143 /* Recompute Z = X cross Y */
144 sgVectorProductVec3 ( z, x, y ) ;
145
146 /* Normalize everything */
147 sgNormaliseVec3 ( x ) ;
148 sgNormaliseVec3 ( y ) ;
149 sgNormaliseVec3 ( z ) ;
150
151 /* Build the matrix */
152 sgSetVec4 ( dst[0], x[0], x[1], x[2], SG_ZERO ) ;
153 sgSetVec4 ( dst[1], y[0], y[1], y[2], SG_ZERO ) ;
154 sgSetVec4 ( dst[2], z[0], z[1], z[2], SG_ZERO ) ;
155 sgSetVec4 ( dst[3], eye[0], eye[1], eye[2], SG_ONE ) ;
156 }
157
158 // -dw- inconsistent linkage!
159
sgTriArea(sgVec3 p0,sgVec3 p1,sgVec3 p2)160 float sgTriArea( sgVec3 p0, sgVec3 p1, sgVec3 p2 )
161 {
162 /*
163 From comp.graph.algorithms FAQ
164
165 2A(P) = abs(N.(sum_{i=0}^{n-1}(v_i x v_{i+1})))
166 This is an optimized version for a triangle
167 but easily extended for planar polygon's with more sides
168 by passing in the number of sides and the vv array
169 sgTriArea( int nsides, float **vv )
170 and changing the normal calculation and the for loop appropriately
171 sgMakeNormal( norm, vv[0], vv[1], vv[2] )
172 for( int i=0; i<n; i++ )
173 */
174
175 sgVec3 sum;
176 sgZeroVec3( sum );
177
178 sgVec3 norm;
179 sgMakeNormal( norm, p0, p1, p2 );
180
181 float *vv[3];
182 vv[0] = p0;
183 vv[1] = p1;
184 vv[2] = p2;
185
186 for( int i=0; i<3; i++ )
187 {
188 int ii = (i+1) % 3;
189
190 sum[0] += (vv[i][1] * vv[ii][2] - vv[i][2] * vv[ii][1]) ;
191 sum[1] += (vv[i][2] * vv[ii][0] - vv[i][0] * vv[ii][2]) ;
192 sum[2] += (vv[i][0] * vv[ii][1] - vv[i][1] * vv[ii][0]) ;
193 }
194
195 float area = sgAbs ( sgScalarProductVec3 ( norm, sum ) ) ;
196
197 return area / 2.0f ;
198 }
199
200 /***************************************************\
201 * functions to get the angle between two vectors *
202 \***************************************************/
203
sgAngleBetweenVec3(sgVec3 v1,sgVec3 v2)204 SGfloat sgAngleBetweenVec3 ( sgVec3 v1, sgVec3 v2 )
205 {
206 sgVec3 nv1, nv2 ;
207
208 sgNormalizeVec3 ( nv1, v1 ) ;
209 sgNormalizeVec3 ( nv2, v2 ) ;
210 return sgAngleBetweenNormalizedVec3 ( nv1, nv2 ) ;
211 }
212
sgAngleBetweenNormalizedVec3(sgVec3 first,sgVec3 second,sgVec3 normal)213 SGfloat sgAngleBetweenNormalizedVec3 (sgVec3 first, sgVec3 second, sgVec3 normal)
214 {
215 // result is in the range 0..360 degrees
216 //
217 // Attention: first and second have to be normalized
218 // the normal is needed to decide between for example 0.123
219 // looking "from one side" and -0.123 looking fomr the other
220
221 SGfloat myCos, abs1, abs2, SProduct, deltaAngle, myNorm;
222
223 if((normal[0]==0) && (normal[1]==0) && (normal[2]==0))
224 {
225 ulSetError ( UL_WARNING, "sgGetAngleBetweenVectors: Normal is zero.");
226 return 0.0 ;
227 }
228
229 sgVec3 temp;
230
231 sgVectorProductVec3( temp, first, second);
232
233 myNorm = sgLengthVec3 ( temp );
234
235 if ( (sgScalarProductVec3(temp, normal))<0 )
236 myNorm = -myNorm;
237
238 if ( myNorm < -0.99999 )
239 deltaAngle = -SG_PI*0.5;
240 else
241 if ( myNorm > 0.99999 )
242 deltaAngle = SG_PI*0.5;
243 else
244 deltaAngle = (SGfloat)asin((double)myNorm);
245
246 // deltaAngle is in the range -SG_PI*0.5 to +SG_PI*0.5 here
247 // However, the correct result could also be
248 // deltaAngleS := pi - deltaAngle
249 // Please note that:
250 // cos(deltaAngleS)=cos(pi-deltaAngle)=-cos(deltaAngle)
251 // So, the question is whether + or - cos(deltaAngle)
252 // is sgScalarProductVec3(first, second)
253
254 if ( deltaAngle < 0 )
255 deltaAngle = deltaAngle + 2*SG_PI; // unnessecary?
256
257 SProduct = sgScalarProductVec3(first, second);
258 myCos = (SGfloat) cos(deltaAngle);
259
260 abs1 = SProduct - myCos;
261 abs2 = SProduct + myCos;
262
263 if ( abs1 < 0 ) abs1 = -abs1 ;
264 if ( abs2 < 0 ) abs2 = -abs2 ;
265
266 assert( (abs1 < 0.1) || (abs2 < 0.1) ) ;
267
268 if ( abs2 < abs1 )
269 {
270 // deltaAngleS is the correct result
271
272 if ( deltaAngle <= SG_PI )
273 deltaAngle = SG_PI - deltaAngle ;
274 else
275 deltaAngle = 3*SG_PI - deltaAngle ;
276 }
277
278 assert ( deltaAngle >= 0.0 ) ;
279 assert ( deltaAngle <= 2.0*SG_PI ) ;
280
281 return deltaAngle * SG_RADIANS_TO_DEGREES ;
282 }
283
284
sgAngleBetweenVec3(sgVec3 v1,sgVec3 v2,sgVec3 normal)285 SGfloat sgAngleBetweenVec3 ( sgVec3 v1, sgVec3 v2, sgVec3 normal )
286 {
287 // nornmal has to be normalized.
288 sgVec3 nv1, nv2 ;
289
290 sgNormalizeVec3 ( nv1, v1 ) ;
291 sgNormalizeVec3 ( nv2, v2 ) ;
292 return sgAngleBetweenNormalizedVec3 ( nv1, nv2, normal ) ;
293 }
294
295
296 /*********************\
297 * sgBox routines *
298 \*********************/
299
300
extend(const sgVec3 v)301 void sgBox::extend ( const sgVec3 v )
302 {
303 if ( isEmpty () )
304 {
305 sgCopyVec3 ( min, v ) ;
306 sgCopyVec3 ( max, v ) ;
307 }
308 else
309 {
310 if ( v[0] < min[0] ) min[0] = v[0] ;
311 if ( v[1] < min[1] ) min[1] = v[1] ;
312 if ( v[2] < min[2] ) min[2] = v[2] ;
313 if ( v[0] > max[0] ) max[0] = v[0] ;
314 if ( v[1] > max[1] ) max[1] = v[1] ;
315 if ( v[2] > max[2] ) max[2] = v[2] ;
316 }
317 }
318
319
extend(const sgBox * b)320 void sgBox::extend ( const sgBox *b )
321 {
322 if ( b -> isEmpty () )
323 return ;
324
325 if ( isEmpty () )
326 {
327 sgCopyVec3 ( min, b->getMin() ) ;
328 sgCopyVec3 ( max, b->getMax() ) ;
329 }
330 else
331 {
332 extend ( b->getMin() ) ;
333 extend ( b->getMax() ) ;
334 }
335 }
336
337
extend(const sgSphere * s)338 void sgBox::extend ( const sgSphere *s )
339 {
340 if ( s -> isEmpty () )
341 return ;
342
343 /*
344 In essence, this extends around a box around the sphere - which
345 is still a perfect solution because both boxes are axially aligned.
346 */
347
348 sgVec3 x ;
349
350 sgSetVec3 ( x, s->getCenter()[0]+s->getRadius(),
351 s->getCenter()[1]+s->getRadius(),
352 s->getCenter()[2]+s->getRadius() ) ;
353 extend ( x ) ;
354
355 sgSetVec3 ( x, s->getCenter()[0]-s->getRadius(),
356 s->getCenter()[1]-s->getRadius(),
357 s->getCenter()[2]-s->getRadius() ) ;
358 extend ( x ) ;
359 }
360
361
intersects(const sgVec4 plane) const362 int sgBox::intersects ( const sgVec4 plane ) const
363 {
364 /*
365 Save multiplies by not redoing Ax+By+Cz+D for each point.
366 */
367
368 SGfloat Ax_min = plane[0] * min[0] ;
369 SGfloat By_min = plane[1] * min[1] ;
370 SGfloat Cz_min_plus_D = plane[2] * min[2] + plane[3] ;
371
372 SGfloat Ax_max = plane[0] * max[0] ;
373 SGfloat By_max = plane[1] * max[1] ;
374 SGfloat Cz_max_plus_D = plane[2] * max[2] + plane[3] ;
375
376 /*
377 Count the number of vertices on the positive side of the plane.
378 */
379
380 int count = ( Ax_min + By_min + Cz_min_plus_D > SG_ZERO ) +
381 ( Ax_min + By_min + Cz_max_plus_D > SG_ZERO ) +
382 ( Ax_min + By_max + Cz_min_plus_D > SG_ZERO ) +
383 ( Ax_min + By_max + Cz_max_plus_D > SG_ZERO ) +
384 ( Ax_max + By_min + Cz_min_plus_D > SG_ZERO ) +
385 ( Ax_max + By_min + Cz_max_plus_D > SG_ZERO ) +
386 ( Ax_max + By_max + Cz_min_plus_D > SG_ZERO ) +
387 ( Ax_max + By_max + Cz_max_plus_D > SG_ZERO ) ;
388
389 /*
390 The plane intersects the box unless all 8 are positive
391 or none of them are positive.
392 */
393
394 return count != 0 && count != 8 ;
395 }
396
397
398
399 /**********************\
400 * sgSphere routines *
401 \**********************/
402
extend(const sgVec3 v)403 void sgSphere::extend ( const sgVec3 v )
404 {
405 if ( isEmpty () )
406 {
407 sgCopyVec3 ( center, v ) ;
408 radius = SG_ZERO ;
409 return ;
410 }
411
412 SGfloat d = sgDistanceVec3 ( center, v ) ;
413
414 if ( d <= radius ) /* Point is already inside sphere */
415 return ;
416
417 SGfloat new_radius = (radius + d) / SG_TWO ; /* Grow radius */
418
419 SGfloat ratio = (new_radius - radius) / d ;
420
421 center[0] += (v[0]-center[0]) * ratio ; /* Move center */
422 center[1] += (v[1]-center[1]) * ratio ;
423 center[2] += (v[2]-center[2]) * ratio ;
424
425 radius = new_radius ;
426 }
427
428
extend(const sgBox * b)429 void sgSphere::extend ( const sgBox *b )
430 {
431 if ( b -> isEmpty () )
432 return ;
433
434 if ( isEmpty() )
435 {
436 sgAddVec3 ( center, b->getMin(), b->getMax() ) ;
437 sgScaleVec3 ( center, SG_HALF ) ;
438 radius = sgDistanceVec3 ( center, b->getMax() ) ;
439 return ;
440 }
441
442 /*
443 I can't think of a faster way to get an
444 utterly minimal sphere.
445
446 The tighter algorithm:- enclose each
447 of eight vertices of the box in turn - it
448 looks like being pretty costly.
449 [8 sqrt()'s]
450
451 The looser algorithm:- enclose the box
452 with an empty sphere and then do a
453 sphere-extend-sphere. This algorithm
454 does well for close-to-cube boxes, but
455 makes very poor spheres for long, thin
456 boxes.
457 [2 sqrt()'s]
458 */
459
460 #ifdef DONT_REALLY_NEED_A_TIGHT_SPHERE_EXTEND_BOX
461
462 /* LOOSER/FASTER sphere-around-sphere-around-box */
463 sgSphere s ;
464 s.empty () ;
465 s.enclose ( b ) ; /* Fast because s is empty */
466 enclose ( s ) ;
467
468 #else
469
470 /* TIGHTER/EXPENSIVE sphere-around-eight-points */
471 sgVec3 x ;
472 extend ( b->getMin() ) ;
473 sgSetVec3 ( x, b->getMin()[0],b->getMin()[1],b->getMax()[2] ) ; extend ( x ) ;
474 sgSetVec3 ( x, b->getMin()[0],b->getMax()[1],b->getMin()[2] ) ; extend ( x ) ;
475 sgSetVec3 ( x, b->getMin()[0],b->getMax()[1],b->getMax()[2] ) ; extend ( x ) ;
476 sgSetVec3 ( x, b->getMax()[0],b->getMin()[1],b->getMin()[2] ) ; extend ( x ) ;
477 sgSetVec3 ( x, b->getMax()[0],b->getMin()[1],b->getMax()[2] ) ; extend ( x ) ;
478 sgSetVec3 ( x, b->getMax()[0],b->getMax()[1],b->getMin()[2] ) ; extend ( x ) ;
479 extend ( b->getMax() ) ;
480 #endif
481 }
482
483
extend(const sgSphere * s)484 void sgSphere::extend ( const sgSphere *s )
485 {
486 if ( s->isEmpty () )
487 return ;
488
489 if ( isEmpty () )
490 {
491 sgCopyVec3 ( center, s->getCenter() ) ;
492 radius = s->getRadius() ;
493 return ;
494 }
495
496 /*
497 d == The distance between the sphere centers
498 */
499
500 SGfloat d = sgDistanceVec3 ( center, s->getCenter() ) ;
501
502 if ( d + s->getRadius() <= radius ) /* New sphere is already inside this one */
503 return ;
504
505 if ( d + radius <= s->getRadius() ) /* New sphere completely contains this one */
506 {
507 sgCopyVec3 ( center, s->getCenter() ) ;
508 radius = s->getRadius() ;
509 return ;
510 }
511
512 /*
513 Build a new sphere that completely contains the other two:
514
515 The center point lies halfway along the line between
516 the furthest points on the edges of the two spheres.
517 Computing those two points is ugly - so we'll use similar
518 triangles
519 */
520
521 SGfloat new_radius = (radius + d + s->getRadius() ) / SG_TWO ;
522
523 SGfloat ratio = ( new_radius - radius ) / d ;
524
525 center[0] += ( s->getCenter()[0] - center[0] ) * ratio ;
526 center[1] += ( s->getCenter()[1] - center[1] ) * ratio ;
527 center[2] += ( s->getCenter()[2] - center[2] ) * ratio ;
528 radius = new_radius ;
529 }
530
531
intersects(const sgBox * b) const532 int sgSphere::intersects ( const sgBox *b ) const
533 {
534 sgVec3 closest ;
535
536 if ( b->getMin()[0] > center[0] ) closest[0] = b->getMin()[0] ; else
537 if ( b->getMax()[0] < center[0] ) closest[0] = b->getMax()[0] ; else
538 closest[0] = center[0] ;
539
540 if ( b->getMin()[1] > center[1] ) closest[1] = b->getMin()[1] ; else
541 if ( b->getMax()[1] < center[1] ) closest[1] = b->getMax()[1] ; else
542 closest[1] = center[1] ;
543
544 if ( b->getMin()[2] > center[2] ) closest[2] = b->getMin()[2] ; else
545 if ( b->getMax()[2] < center[2] ) closest[2] = b->getMax()[2] ; else
546 closest[2] = center[2] ;
547
548 return sgCompare3DSqdDist ( closest, center, sgSquare ( radius ) ) <= 0 ;
549 }
550
551
552 /************************\
553 * sgFrustum routines *
554 \************************/
555
update()556 void sgFrustum::update ()
557 {
558 if ( fabs ( ffar - nnear ) < 0.1 )
559 {
560 ulSetError ( UL_WARNING, "sgFrustum: Can't support depth of view <0.1 units.");
561 return ;
562 }
563
564 if ( hfov != SG_ZERO && vfov != SG_ZERO )
565 {
566 if ( fabs ( hfov ) < 0.1 || fabs ( vfov ) < 0.1 )
567 {
568 ulSetError ( UL_WARNING, ortho ?
569 "sgFrustum: Can't support width or height <0.1 units." :
570 "sgFrustum: Can't support fields of view narrower than 0.1 degrees." ) ;
571 return ;
572 }
573
574 if ( ortho )
575 {
576 right = SG_HALF * hfov ;
577 top = SG_HALF * vfov ;
578 }
579 else
580 {
581 right = nnear * (SGfloat) tan ( hfov * SG_DEGREES_TO_RADIANS / SG_TWO ) ;
582 top = nnear * (SGfloat) tan ( vfov * SG_DEGREES_TO_RADIANS / SG_TWO ) ;
583 }
584
585 left = -right ;
586 bot = -top ;
587 }
588
589
590 /* Compute the projection matrix */
591
592 SGfloat width = right - left ;
593 SGfloat height = top - bot ;
594 SGfloat depth = ffar - nnear ;
595
596 if ( ortho )
597 {
598 /* orthographic */
599
600 mat[0][0] = SG_TWO / width ;
601 mat[0][1] = SG_ZERO ;
602 mat[0][2] = SG_ZERO ;
603 mat[0][3] = SG_ZERO ;
604
605 mat[1][0] = SG_ZERO ;
606 mat[1][1] = SG_TWO / height ;
607 mat[1][2] = SG_ZERO ;
608 mat[1][3] = SG_ZERO ;
609
610 mat[2][0] = SG_ZERO ;
611 mat[2][1] = SG_ZERO ;
612 mat[2][2] = -SG_TWO / depth ;
613 mat[2][3] = SG_ZERO ;
614
615 mat[3][0] = -( left + right ) / width ;
616 mat[3][1] = -( bot + top ) / height ;
617 mat[3][2] = -( nnear + ffar ) / depth ;
618 mat[3][3] = SG_ONE ;
619 }
620 else
621 {
622 /* perspective */
623
624 mat[0][0] = SG_TWO * nnear / width ;
625 mat[0][1] = SG_ZERO ;
626 mat[0][2] = SG_ZERO ;
627 mat[0][3] = SG_ZERO ;
628
629 mat[1][0] = SG_ZERO ;
630 mat[1][1] = SG_TWO * nnear / height ;
631 mat[1][2] = SG_ZERO ;
632 mat[1][3] = SG_ZERO ;
633
634 mat[2][0] = ( right + left ) / width ;
635 mat[2][1] = ( top + bot ) / height ;
636 mat[2][2] = -( ffar + nnear ) / depth ;
637 mat[2][3] = -SG_ONE ;
638
639 mat[3][0] = SG_ZERO ;
640 mat[3][1] = SG_ZERO ;
641 mat[3][2] = -SG_TWO * nnear * ffar / depth ;
642 mat[3][3] = SG_ZERO ;
643 }
644
645
646 /*
647 * The clip planes are derived from the projection matrix.
648 *
649 * After projection (in clip coordinates), the clip planes are simply:
650 *
651 * left: ( 1, 0, 0, 1 )
652 * right: ( -1, 0, 0, 1 )
653 * bottom: ( 0, 1, 0, 1 )
654 * top: ( 0, -1, 0, 1 )
655 * near: ( 0, 0, 1, 1 )
656 * far: ( 0, 0, -1, 1 )
657 *
658 * These can easily be transformed *backwards* by
659 * multiplying by the transposed projection matrix, i.e:
660 *
661 * ( A ) ( A')
662 * ( B ) = mat^T ( B')
663 * ( C ) ( C')
664 * ( D ) ( D')
665 *
666 * where (A',B',C',D') represents a plane in clip coordinates,
667 * and (A,B,C,D) is the same plane expressed in eye coordinates.
668 */
669
670 sgSetVec4( plane[ SG_LEFT_PLANE ], SG_ONE, SG_ZERO, SG_ZERO, SG_ONE );
671 sgSetVec4( plane[ SG_RIGHT_PLANE ], -SG_ONE, SG_ZERO, SG_ZERO, SG_ONE );
672 sgSetVec4( plane[ SG_BOT_PLANE ], SG_ZERO, SG_ONE, SG_ZERO, SG_ONE );
673 sgSetVec4( plane[ SG_TOP_PLANE ], SG_ZERO, -SG_ONE, SG_ZERO, SG_ONE );
674 sgSetVec4( plane[ SG_NEAR_PLANE ], SG_ZERO, SG_ZERO, SG_ONE, SG_ONE );
675 sgSetVec4( plane[ SG_FAR_PLANE ], SG_ZERO, SG_ZERO, -SG_ONE, SG_ONE );
676
677 for ( int i = 0 ; i < 6 ; i++ )
678 {
679 sgVec4 tmp ;
680
681 for ( int j = 0 ; j < 4 ; j++ )
682 tmp[j] = sgScalarProductVec4 ( plane[i], mat[j] ) ;
683
684 sgScaleVec4 ( plane[i], tmp, SG_ONE / sgLengthVec3 ( tmp ) ) ;
685 }
686 }
687
688
689
690 #define OC_LEFT_SHIFT 0
691 #define OC_RIGHT_SHIFT 1
692 #define OC_TOP_SHIFT 2
693 #define OC_BOT_SHIFT 3
694 #define OC_NEAR_SHIFT 4
695 #define OC_FAR_SHIFT 5
696
697 #define OC_ALL_ON_SCREEN 0x3F
698 #define OC_OFF_TRF ((1<<OC_TOP_SHIFT)|(1<<OC_RIGHT_SHIFT)|(1<<OC_FAR_SHIFT))
699 #define OC_OFF_BLN ((1<<OC_BOT_SHIFT)|(1<<OC_LEFT_SHIFT)|(1<<OC_NEAR_SHIFT))
700
getOutcode(const sgVec3 pt) const701 int sgFrustum::getOutcode ( const sgVec3 pt ) const
702 {
703 /* Transform the point by the Frustum's transform. */
704
705 sgVec4 tmp ;
706
707 tmp [ 0 ] = pt [ 0 ] ;
708 tmp [ 1 ] = pt [ 1 ] ;
709 tmp [ 2 ] = pt [ 2 ] ;
710 tmp [ 3 ] = SG_ONE ;
711
712 sgXformPnt4 ( tmp, tmp, mat ) ;
713
714 /*
715 No need to divide by the 'w' component since we are only checking for
716 results in the range 0..1
717 */
718
719 return (( tmp[0] <= tmp[3] ) << OC_RIGHT_SHIFT ) |
720 (( tmp[0] >= -tmp[3] ) << OC_LEFT_SHIFT ) |
721 (( tmp[1] <= tmp[3] ) << OC_TOP_SHIFT ) |
722 (( tmp[1] >= -tmp[3] ) << OC_BOT_SHIFT ) |
723 (( tmp[2] <= tmp[3] ) << OC_FAR_SHIFT ) |
724 (( tmp[2] >= -tmp[3] ) << OC_NEAR_SHIFT ) ;
725 }
726
contains(const sgVec3 pt) const727 int sgFrustum::contains ( const sgVec3 pt ) const
728 {
729 return getOutcode ( pt ) == OC_ALL_ON_SCREEN ;
730 }
731
732
contains(const sgSphere * s) const733 int sgFrustum::contains ( const sgSphere *s ) const
734 {
735
736 const SGfloat *center = s->getCenter() ;
737 const SGfloat radius = s->getRadius() ;
738
739 /*
740 Lop off half the database (roughly) with a quick near-plane test - and
741 lop off a lot more with a quick far-plane test
742 */
743
744 if ( -center[2] + radius < nnear || -center[2] - radius > ffar )
745 return SG_OUTSIDE ;
746
747 /*
748 OK, so the sphere lies between near and far.
749
750 Measure the distance of the center point from the four sides of the frustum,
751 if it's outside by more than the radius then it's history.
752
753 It's tempting to do a quick test to see if the center point is
754 onscreen using sgFrustumContainsPt - but that takes a matrix transform
755 which is 16 multiplies and 12 adds - versus this test which does the
756 whole task using only 12 multiplies and 8 adds.
757 */
758
759 /*
760 A few operations are saved by observing that certain values in the plane
761 equations are zero or one. These are specific to orthographic and perspective
762 projections respectively.
763 */
764
765 SGfloat sp1, sp2, sp3, sp4 ;
766
767 if ( ortho )
768 {
769 /*
770 left: ( 1, 0, 0, x )
771 right: ( -1, 0, 0, x )
772 bottom: ( 0, 1, 0, x )
773 top: ( 0, -1, 0, x )
774 */
775 sp1 = plane[ SG_LEFT_PLANE ][3] + center[0] ;
776 sp2 = plane[ SG_RIGHT_PLANE ][3] - center[0] ;
777 sp3 = plane[ SG_BOT_PLANE ][3] + center[1] ;
778 sp4 = plane[ SG_TOP_PLANE ][3] - center[1] ;
779 }
780 else
781 {
782 /*
783 left: ( x, 0, x, 0 )
784 right: ( x, 0, x, 0 )
785 bottom: ( 0, x, x, 0 )
786 top: ( 0, x, x, 0 )
787 */
788 sp1 = plane[ SG_LEFT_PLANE ][0] * center[0] + plane[ SG_LEFT_PLANE ][2] * center[2] ;
789 sp2 = plane[ SG_RIGHT_PLANE ][0] * center[0] + plane[ SG_RIGHT_PLANE ][2] * center[2] ;
790 sp3 = plane[ SG_BOT_PLANE ][1] * center[1] + plane[ SG_BOT_PLANE ][2] * center[2] ;
791 sp4 = plane[ SG_TOP_PLANE ][1] * center[1] + plane[ SG_TOP_PLANE ][2] * center[2] ;
792 }
793
794 /*
795 Note: in the general case, we would have to do:
796
797 sp1 = sgScalarProductVec3 ( left_plane, center ) + left_plane[3] ;
798 sp2 = sgScalarProductVec3 ( right_plane, center ) + right_plane[3] ;
799 ...
800 sp6 = sgScalarProductVec3 ( far_plane, center ) + far_plane[3] ;
801 */
802
803
804 if ( -sp1 > radius || -sp2 > radius || -sp3 > radius || -sp4 > radius )
805 return SG_OUTSIDE ;
806
807 /*
808 If it's inside by more than the radius then it's *completely* inside
809 and we can save time elsewhere if we know that for sure.
810 */
811
812 if ( sp1 >= radius && sp2 >= radius && sp3 >= radius && sp4 >= radius
813 && -center[2] - radius >= nnear && -center[2] + radius <= ffar )
814 return SG_INSIDE ;
815
816 return SG_STRADDLE ;
817 }
818
819
contains(const sgBox * b) const820 int sgFrustum::contains ( const sgBox *b ) const
821 {
822 sgVec3 p[8] = {
823 { b->getMin()[0], b->getMin()[1], b->getMin()[2] },
824 { b->getMax()[0], b->getMin()[1], b->getMin()[2] },
825 { b->getMin()[0], b->getMax()[1], b->getMin()[2] },
826 { b->getMax()[0], b->getMax()[1], b->getMin()[2] },
827 { b->getMin()[0], b->getMin()[1], b->getMax()[2] },
828 { b->getMax()[0], b->getMin()[1], b->getMax()[2] },
829 { b->getMin()[0], b->getMax()[1], b->getMax()[2] },
830 { b->getMax()[0], b->getMax()[1], b->getMax()[2] },
831 } ;
832
833 int all = -1 ;
834 int one = 0 ;
835
836 for (int i = 0 ; i < 8 ; i++ )
837 {
838 int tmp = ~ getOutcode ( p[i] ) ;
839 all &= tmp ;
840 one |= tmp ;
841 }
842
843 return ( all ? SG_OUTSIDE : one ? SG_STRADDLE : SG_INSIDE ) ;
844 }
845
846
847
sgDistSquaredToLineVec3(const sgLine3 line,const sgVec3 pnt)848 SGfloat sgDistSquaredToLineVec3 ( const sgLine3 line, const sgVec3 pnt )
849 {
850 sgVec3 r ; sgSubVec3 ( r, pnt, line.point_on_line ) ;
851
852 float projectedDistance = sgScalarProductVec3(r,line.direction_vector);
853
854 return sgScalarProductVec3 ( r, r ) -
855 projectedDistance * projectedDistance;
856 }
857
858
859
sgDistSquaredToLineSegmentVec3(const sgLineSegment3 line,const sgVec3 pnt)860 SGfloat sgDistSquaredToLineSegmentVec3 ( const sgLineSegment3 line,
861 const sgVec3 pnt )
862 {
863 sgVec3 v ; sgSubVec3 ( v, line.b, line.a ) ;
864 sgVec3 r1 ; sgSubVec3 ( r1, pnt, line.a ) ;
865
866 SGfloat r1_dot_v = sgScalarProductVec3 ( r1, v ) ;
867
868 if ( r1_dot_v <= 0 ) /* Off the "A" end */
869 return sgScalarProductVec3 ( r1, r1 ) ;
870
871 sgVec3 r2 ; sgSubVec3 ( r2, pnt, line.b ) ;
872
873 SGfloat r2_dot_v = sgScalarProductVec3 ( r2, v ) ;
874
875 if ( r2_dot_v >= 0 ) /* Off the "B" end */
876 return sgScalarProductVec3 ( r2, r2 ) ;
877
878 /* Closest point on line is on the line segment */
879
880 return sgScalarProductVec3 ( r1, r1 ) - r1_dot_v * r1_dot_v / sgScalarProductVec3 ( v, v ) ;
881 }
882
883
884
sgMakeCoordMat4(sgMat4 m,const SGfloat x,const SGfloat y,const SGfloat z,const SGfloat h,const SGfloat p,const SGfloat r)885 void sgMakeCoordMat4 ( sgMat4 m, const SGfloat x, const SGfloat y, const SGfloat z, const SGfloat h, const SGfloat p, const SGfloat r )
886 {
887 SGfloat ch, sh, cp, sp, cr, sr, srsp, crsp, srcp ;
888
889 if ( h == SG_ZERO )
890 {
891 ch = SGD_ONE ;
892 sh = SGD_ZERO ;
893 }
894 else
895 {
896 sh = sgSin ( h ) ;
897 ch = sgCos ( h ) ;
898 }
899
900 if ( p == SG_ZERO )
901 {
902 cp = SGD_ONE ;
903 sp = SGD_ZERO ;
904 }
905 else
906 {
907 sp = sgSin ( p ) ;
908 cp = sgCos ( p ) ;
909 }
910
911 if ( r == SG_ZERO )
912 {
913 cr = SGD_ONE ;
914 sr = SGD_ZERO ;
915 srsp = SGD_ZERO ;
916 srcp = SGD_ZERO ;
917 crsp = sp ;
918 }
919 else
920 {
921 sr = sgSin ( r ) ;
922 cr = sgCos ( r ) ;
923 srsp = sr * sp ;
924 crsp = cr * sp ;
925 srcp = sr * cp ;
926 }
927
928 m[0][0] = (SGfloat)( ch * cr - sh * srsp ) ;
929 m[1][0] = (SGfloat)( -sh * cp ) ;
930 m[2][0] = (SGfloat)( sr * ch + sh * crsp ) ;
931 m[3][0] = x ;
932
933 m[0][1] = (SGfloat)( cr * sh + srsp * ch ) ;
934 m[1][1] = (SGfloat)( ch * cp ) ;
935 m[2][1] = (SGfloat)( sr * sh - crsp * ch ) ;
936 m[3][1] = y ;
937
938 m[0][2] = (SGfloat)( -srcp ) ;
939 m[1][2] = (SGfloat)( sp ) ;
940 m[2][2] = (SGfloat)( cr * cp ) ;
941 m[3][2] = z ;
942
943 m[0][3] = SG_ZERO ;
944 m[1][3] = SG_ZERO ;
945 m[2][3] = SG_ZERO ;
946 m[3][3] = SG_ONE ;
947 }
948
949
sgMakeTransMat4(sgMat4 m,const sgVec3 xyz)950 void sgMakeTransMat4 ( sgMat4 m, const sgVec3 xyz )
951 {
952 m[0][1] = m[0][2] = m[0][3] =
953 m[1][0] = m[1][2] = m[1][3] =
954 m[2][0] = m[2][1] = m[2][3] = SG_ZERO ;
955 m[0][0] = m[1][1] = m[2][2] = m[3][3] = SG_ONE ;
956 sgCopyVec3 ( m[3], xyz ) ;
957 }
958
959
sgMakeTransMat4(sgMat4 m,const SGfloat x,const SGfloat y,const SGfloat z)960 void sgMakeTransMat4 ( sgMat4 m, const SGfloat x, const SGfloat y, const SGfloat z )
961 {
962 m[0][1] = m[0][2] = m[0][3] =
963 m[1][0] = m[1][2] = m[1][3] =
964 m[2][0] = m[2][1] = m[2][3] = SG_ZERO ;
965 m[0][0] = m[1][1] = m[2][2] = m[3][3] = SG_ONE ;
966 sgSetVec3 ( m[3], x, y, z ) ;
967 }
968
969
sgSetCoord(sgCoord * dst,const sgMat4 src)970 void sgSetCoord ( sgCoord *dst, const sgMat4 src )
971 {
972 sgCopyVec3 ( dst->xyz, src[3] ) ;
973
974 sgMat4 mat ;
975
976 SGfloat s = sgLengthVec3 ( src[0] ) ;
977
978 if ( s <= 0.00001 )
979 {
980 ulSetError ( UL_WARNING, "sgMat4ToCoord: ERROR - Bad Matrix." ) ;
981 sgSetVec3 ( dst -> hpr, SG_ZERO, SG_ZERO, SG_ZERO ) ;
982 return ;
983 }
984
985 sgScaleMat4 ( mat, src, SG_ONE / s ) ;
986
987 dst->hpr[1] = sgASin ( _sgClampToUnity ( mat[1][2] ) ) ;
988
989 SGfloat cp = sgCos ( dst->hpr[1] ) ;
990
991 /* If pointing nearly vertically up - then heading is ill-defined */
992
993 if ( cp > -0.00001 && cp < 0.00001 )
994 {
995 SGfloat cr = _sgClampToUnity ( mat[0][1] ) ;
996 SGfloat sr = _sgClampToUnity (-mat[2][1] ) ;
997
998 dst->hpr[0] = SG_ZERO ;
999 dst->hpr[2] = sgATan2 ( sr, cr ) ;
1000 }
1001 else
1002 {
1003 cp = SG_ONE / cp ;
1004 SGfloat sr = _sgClampToUnity ( -mat[0][2] * cp ) ;
1005 SGfloat cr = _sgClampToUnity ( mat[2][2] * cp ) ;
1006 SGfloat sh = _sgClampToUnity ( -mat[1][0] * cp ) ;
1007 SGfloat ch = _sgClampToUnity ( mat[1][1] * cp ) ;
1008
1009 if ( (sh == SG_ZERO && ch == SG_ZERO) || (sr == SG_ZERO && cr == SG_ZERO) )
1010 {
1011 cr = _sgClampToUnity ( mat[0][1] ) ;
1012 sr = _sgClampToUnity (-mat[2][1] ) ;
1013
1014 dst->hpr[0] = SG_ZERO ;
1015 }
1016 else
1017 dst->hpr[0] = sgATan2 ( sh, ch ) ;
1018
1019 dst->hpr[2] = sgATan2 ( sr, cr ) ;
1020 }
1021 }
1022
1023
sgMakeNormal(sgVec2 dst,const sgVec2 a,const sgVec2 b)1024 void sgMakeNormal(sgVec2 dst, const sgVec2 a, const sgVec2 b )
1025 {
1026 sgSubVec2 ( dst, b, a ) ;
1027 float tmp = dst [ 0 ] ; dst [ 0 ] = -dst [ 1 ] ; dst [ 1 ] = tmp ;
1028 sgNormaliseVec2 ( dst ) ;
1029 }
1030
1031
sgMakeNormal(sgVec3 dst,const sgVec3 a,const sgVec3 b,const sgVec3 c)1032 void sgMakeNormal(sgVec3 dst, const sgVec3 a, const sgVec3 b, const sgVec3 c )
1033 {
1034 sgVec3 ab ; sgSubVec3 ( ab, b, a ) ;
1035 sgVec3 ac ; sgSubVec3 ( ac, c, a ) ;
1036 sgVectorProductVec3 ( dst, ab,ac ) ; sgNormaliseVec3 ( dst ) ;
1037 }
1038
1039
sgPreMultMat4(sgMat4 dst,const sgMat4 src)1040 void sgPreMultMat4( sgMat4 dst, const sgMat4 src )
1041 {
1042 sgMat4 mat ;
1043 sgMultMat4 ( mat, dst, src ) ;
1044 sgCopyMat4 ( dst, mat ) ;
1045 }
1046
sgPostMultMat4(sgMat4 dst,const sgMat4 src)1047 void sgPostMultMat4( sgMat4 dst, const sgMat4 src )
1048 {
1049 sgMat4 mat ;
1050 sgMultMat4 ( mat, src, dst ) ;
1051 sgCopyMat4 ( dst, mat ) ;
1052 }
1053
sgMultMat4(sgMat4 dst,const sgMat4 m1,const sgMat4 m2)1054 void sgMultMat4( sgMat4 dst, const sgMat4 m1, const sgMat4 m2 )
1055 {
1056 for ( int j = 0 ; j < 4 ; j++ )
1057 {
1058 dst[0][j] = m2[0][0] * m1[0][j] +
1059 m2[0][1] * m1[1][j] +
1060 m2[0][2] * m1[2][j] +
1061 m2[0][3] * m1[3][j] ;
1062
1063 dst[1][j] = m2[1][0] * m1[0][j] +
1064 m2[1][1] * m1[1][j] +
1065 m2[1][2] * m1[2][j] +
1066 m2[1][3] * m1[3][j] ;
1067
1068 dst[2][j] = m2[2][0] * m1[0][j] +
1069 m2[2][1] * m1[1][j] +
1070 m2[2][2] * m1[2][j] +
1071 m2[2][3] * m1[3][j] ;
1072
1073 dst[3][j] = m2[3][0] * m1[0][j] +
1074 m2[3][1] * m1[1][j] +
1075 m2[3][2] * m1[2][j] +
1076 m2[3][3] * m1[3][j] ;
1077 }
1078 }
1079
1080
sgTransposeNegateMat4(sgMat4 dst,const sgMat4 src)1081 void sgTransposeNegateMat4 ( sgMat4 dst, const sgMat4 src )
1082 {
1083 /* Poor man's invert - can be used when matrix is a simple rotate-translate */
1084
1085 dst[0][0] = src[0][0] ;
1086 dst[1][0] = src[0][1] ;
1087 dst[2][0] = src[0][2] ;
1088 dst[3][0] = - sgScalarProductVec3 ( src[3], src[0] ) ;
1089
1090 dst[0][1] = src[1][0] ;
1091 dst[1][1] = src[1][1] ;
1092 dst[2][1] = src[1][2] ;
1093 dst[3][1] = - sgScalarProductVec3 ( src[3], src[1] ) ;
1094
1095 dst[0][2] = src[2][0] ;
1096 dst[1][2] = src[2][1] ;
1097 dst[2][2] = src[2][2] ;
1098 dst[3][2] = - sgScalarProductVec3 ( src[3], src[2] ) ;
1099
1100 dst[0][3] = SG_ZERO ;
1101 dst[1][3] = SG_ZERO ;
1102 dst[2][3] = SG_ZERO ;
1103 dst[3][3] = SG_ONE ;
1104 }
1105
1106
sgTransposeNegateMat4(sgMat4 dst)1107 void sgTransposeNegateMat4 ( sgMat4 dst )
1108 {
1109 sgMat4 src ;
1110 sgCopyMat4 ( src, dst ) ;
1111 sgTransposeNegateMat4 ( dst, src ) ;
1112 }
1113
1114
1115
sgInvertMat4(sgMat4 dst,const sgMat4 src)1116 void sgInvertMat4 ( sgMat4 dst, const sgMat4 src )
1117 {
1118 sgMat4 tmp ;
1119
1120 sgCopyMat4 ( tmp, src ) ;
1121 sgMakeIdentMat4 ( dst ) ;
1122
1123 for ( int i = 0 ; i != 4 ; i++ )
1124 {
1125 SGfloat val = tmp[i][i] ;
1126 int ind = i ;
1127 int j ;
1128
1129 for ( j = i + 1 ; j != 4 ; j++ )
1130 {
1131 if ( fabs ( tmp[i][j] ) > fabs(val) )
1132 {
1133 ind = j;
1134 val = tmp[i][j] ;
1135 }
1136 }
1137
1138 if ( ind != i )
1139 { /* swap columns */
1140 for ( j = 0 ; j != 4 ; j++ )
1141 {
1142 SGfloat t ;
1143 t = dst[j][i]; dst[j][i] = dst[j][ind]; dst[j][ind] = t ;
1144 t = tmp[j][i]; tmp[j][i] = tmp[j][ind]; tmp[j][ind] = t ;
1145 }
1146 }
1147
1148 // if ( val == SG_ZERO)
1149 if ( fabs(val) <= FLT_EPSILON )
1150 {
1151 ulSetError ( UL_WARNING, "sg: ERROR - Singular matrix, no inverse!" ) ;
1152 sgMakeIdentMat4 ( dst ) ; /* Do *something* */
1153 return;
1154 }
1155
1156 SGfloat ival = SG_ONE / val ;
1157
1158 for ( j = 0 ; j != 4 ; j++ )
1159 {
1160 tmp[j][i] *= ival ;
1161 dst[j][i] *= ival ;
1162 }
1163
1164 for (j = 0; j != 4; j++)
1165 {
1166 if ( j == i )
1167 continue ;
1168
1169 val = tmp[i][j] ;
1170
1171 for ( int k = 0 ; k != 4 ; k++ )
1172 {
1173 tmp[k][j] -= tmp[k][i] * val ;
1174 dst[k][j] -= dst[k][i] * val ;
1175 }
1176 }
1177 }
1178 }
1179
1180
1181
sgXformVec3(sgVec3 dst,const sgVec3 src,const sgMat4 mat)1182 void sgXformVec3 ( sgVec3 dst, const sgVec3 src, const sgMat4 mat )
1183 {
1184 SGfloat t0 = src[ 0 ] ;
1185 SGfloat t1 = src[ 1 ] ;
1186 SGfloat t2 = src[ 2 ] ;
1187
1188 dst[0] = t0 * mat[ 0 ][ 0 ] +
1189 t1 * mat[ 1 ][ 0 ] +
1190 t2 * mat[ 2 ][ 0 ] ;
1191
1192 dst[1] = t0 * mat[ 0 ][ 1 ] +
1193 t1 * mat[ 1 ][ 1 ] +
1194 t2 * mat[ 2 ][ 1 ] ;
1195
1196 dst[2] = t0 * mat[ 0 ][ 2 ] +
1197 t1 * mat[ 1 ][ 2 ] +
1198 t2 * mat[ 2 ][ 2 ] ;
1199 }
1200
1201
sgXformPnt3(sgVec3 dst,const sgVec3 src,const sgMat4 mat)1202 void sgXformPnt3 ( sgVec3 dst, const sgVec3 src, const sgMat4 mat )
1203 {
1204 SGfloat t0 = src[ 0 ] ;
1205 SGfloat t1 = src[ 1 ] ;
1206 SGfloat t2 = src[ 2 ] ;
1207
1208 dst[0] = t0 * mat[ 0 ][ 0 ] +
1209 t1 * mat[ 1 ][ 0 ] +
1210 t2 * mat[ 2 ][ 0 ] +
1211 mat[ 3 ][ 0 ] ;
1212
1213 dst[1] = t0 * mat[ 0 ][ 1 ] +
1214 t1 * mat[ 1 ][ 1 ] +
1215 t2 * mat[ 2 ][ 1 ] +
1216 mat[ 3 ][ 1 ] ;
1217
1218 dst[2] = t0 * mat[ 0 ][ 2 ] +
1219 t1 * mat[ 1 ][ 2 ] +
1220 t2 * mat[ 2 ][ 2 ] +
1221 mat[ 3 ][ 2 ] ;
1222 }
1223
1224
sgXformPnt4(sgVec4 dst,const sgVec4 src,const sgMat4 mat)1225 void sgXformPnt4 ( sgVec4 dst, const sgVec4 src, const sgMat4 mat )
1226 {
1227 SGfloat t0 = src[ 0 ] ;
1228 SGfloat t1 = src[ 1 ] ;
1229 SGfloat t2 = src[ 2 ] ;
1230 SGfloat t3 = src[ 3 ] ;
1231
1232 dst[0] = t0 * mat[ 0 ][ 0 ] +
1233 t1 * mat[ 1 ][ 0 ] +
1234 t2 * mat[ 2 ][ 0 ] +
1235 t3 * mat[ 3 ][ 0 ] ;
1236
1237 dst[1] = t0 * mat[ 0 ][ 1 ] +
1238 t1 * mat[ 1 ][ 1 ] +
1239 t2 * mat[ 2 ][ 1 ] +
1240 t3 * mat[ 3 ][ 1 ] ;
1241
1242 dst[2] = t0 * mat[ 0 ][ 2 ] +
1243 t1 * mat[ 1 ][ 2 ] +
1244 t2 * mat[ 2 ][ 2 ] +
1245 t3 * mat[ 3 ][ 2 ] ;
1246
1247 dst[3] = t0 * mat[ 0 ][ 3 ] +
1248 t1 * mat[ 1 ][ 3 ] +
1249 t2 * mat[ 2 ][ 3 ] +
1250 t3 * mat[ 3 ][ 3 ] ;
1251 }
1252
1253
sgFullXformPnt3(sgVec3 dst,const sgVec3 src,const sgMat4 mat)1254 void sgFullXformPnt3 ( sgVec3 dst, const sgVec3 src, const sgMat4 mat )
1255 {
1256 sgVec4 tmp ;
1257
1258 tmp [ 0 ] = src [ 0 ] ;
1259 tmp [ 1 ] = src [ 1 ] ;
1260 tmp [ 2 ] = src [ 2 ] ;
1261 tmp [ 3 ] = SG_ONE ;
1262
1263 sgXformPnt4 ( tmp, tmp, mat ) ;
1264 sgScaleVec3 ( dst, tmp, SG_ONE / tmp [ 3 ] ) ;
1265 }
1266
sgHPRfromVec3(sgVec3 hpr,const sgVec3 src)1267 void sgHPRfromVec3 ( sgVec3 hpr, const sgVec3 src )
1268 {
1269 sgVec3 tmp ;
1270 sgCopyVec3 ( tmp, src ) ;
1271 sgNormaliseVec3 ( tmp ) ;
1272 hpr[0] = - sgATan2 ( tmp [ 0 ], tmp [ 1 ] ) ;
1273 hpr[1] = - sgATan2 ( tmp [ 2 ], sgSqrt ( sgSquare ( tmp [ 0 ] ) +
1274 sgSquare ( tmp [ 1 ] ) ) ) ;
1275 hpr[2] = SG_ZERO ;
1276 }
1277
1278
1279
1280 /*
1281 Quaternion routines are Copyright (C) 1999
1282 Kevin B. Thompson <kevinbthompson@yahoo.com>
1283 Modified by Sylvan W. Clebsch <sylvan@stanford.edu>
1284 Largely rewritten by "Negative0" <negative0@earthlink.net>
1285 More added by John Fay.
1286 */
1287
1288
sgQuatToAngleAxis(SGfloat * angle,SGfloat * x,SGfloat * y,SGfloat * z,const sgQuat src)1289 void sgQuatToAngleAxis ( SGfloat *angle,
1290 SGfloat *x, SGfloat *y, SGfloat *z,
1291 const sgQuat src )
1292 {
1293 sgVec3 axis ;
1294
1295 sgQuatToAngleAxis ( angle, axis, src ) ;
1296
1297 *x = axis [ 0 ] ;
1298 *y = axis [ 1 ] ;
1299 *z = axis [ 2 ] ;
1300 }
1301
1302
sgQuatToAngleAxis(SGfloat * angle,sgVec3 axis,const sgQuat src)1303 void sgQuatToAngleAxis ( SGfloat *angle, sgVec3 axis, const sgQuat src )
1304 {
1305 SGfloat a = (SGfloat) acos ( src[SG_W] ) ;
1306 SGfloat s = (SGfloat) sin ( a ) ;
1307
1308 *angle = a * SG_RADIANS_TO_DEGREES * SG_TWO ;
1309
1310 if ( s == SG_ZERO )
1311 sgSetVec3 ( axis, SG_ZERO, SG_ZERO, SG_ONE );
1312 else
1313 {
1314 sgSetVec3 ( axis, src[SG_X], src[SG_Y], src[SG_Z] ) ;
1315 sgScaleVec3 ( axis, SG_ONE / s ) ;
1316 }
1317 }
1318
1319
sgAngleAxisToQuat(sgQuat dst,const SGfloat angle,const SGfloat x,const SGfloat y,const SGfloat z)1320 void sgAngleAxisToQuat ( sgQuat dst,
1321 const SGfloat angle,
1322 const SGfloat x, const SGfloat y, const SGfloat z )
1323 {
1324 sgVec3 axis ;
1325 sgSetVec3 ( axis, x, y, z ) ;
1326 sgAngleAxisToQuat ( dst, angle, axis ) ;
1327 }
1328
1329
sgAngleAxisToQuat(sgQuat dst,const SGfloat angle,const sgVec3 axis)1330 void sgAngleAxisToQuat ( sgQuat dst, const SGfloat angle, const sgVec3 axis )
1331 {
1332 SGfloat temp_angle = angle * SG_DEGREES_TO_RADIANS / SG_TWO ;
1333
1334 sgVec3 ax ;
1335 sgNormaliseVec3 ( ax, axis ) ;
1336
1337 SGfloat s = - (SGfloat) sin ( temp_angle ) ;
1338
1339 dst[SG_W] = (SGfloat) cos ( temp_angle ) ;
1340 sgScaleVec3 ( dst, ax, s ) ;
1341 }
1342
1343
1344 //from gamasutra.com
1345 //by nb
1346
sgMatrixToQuat(sgQuat quat,const sgMat4 m)1347 void sgMatrixToQuat( sgQuat quat, const sgMat4 m )
1348 {
1349 SGfloat tr, s, q[4] ;
1350 int i, j, k ;
1351
1352 int nxt[3] = {1, 2, 0};
1353
1354 tr = m[0][0] + m[1][1] + m[2][2];
1355
1356 // check the diagonal
1357 if (tr > SG_ZERO )
1358 {
1359 s = (SGfloat) sgSqrt (tr + SG_ONE);
1360 quat[SG_W] = s / SG_TWO;
1361 s = SG_HALF / s;
1362 quat[SG_X] = (m[1][2] - m[2][1]) * s;
1363 quat[SG_Y] = (m[2][0] - m[0][2]) * s;
1364 quat[SG_Z] = (m[0][1] - m[1][0]) * s;
1365 }
1366 else
1367 {
1368 // diagonal is negative
1369 i = 0;
1370 if (m[1][1] > m[0][0]) i = 1;
1371 if (m[2][2] > m[i][i]) i = 2;
1372 j = nxt[i];
1373 k = nxt[j];
1374 s = (SGfloat) sgSqrt ((m[i][i] - (m[j][j] + m[k][k])) + SG_ONE);
1375 q[i] = s * SG_HALF;
1376
1377 if (s != SG_ZERO) s = SG_HALF / s;
1378
1379 q[3] = (m[j][k] - m[k][j]) * s;
1380 q[j] = (m[i][j] + m[j][i]) * s;
1381 q[k] = (m[i][k] + m[k][i]) * s;
1382
1383 quat[SG_X] = q[0];
1384 quat[SG_Y] = q[1];
1385 quat[SG_Z] = q[2];
1386 quat[SG_W] = q[3];
1387 }
1388
1389 // seems to yield the inverse rotation, so:
1390 quat[SG_W] = - quat[SG_W];
1391 }
1392
1393
sgMultQuat(sgQuat dst,const sgQuat a,const sgQuat b)1394 void sgMultQuat ( sgQuat dst, const sgQuat a, const sgQuat b )
1395 {
1396 /* [ ww' - v.v', vxv' + wv' + v'w ] */
1397
1398 SGfloat t[8];
1399
1400 t[0] = (a[SG_W] + a[SG_X]) * (b[SG_W] + b[SG_X]);
1401 t[1] = (a[SG_Z] - a[SG_Y]) * (b[SG_Y] - b[SG_Z]);
1402 t[2] = (a[SG_X] - a[SG_W]) * (b[SG_Y] + b[SG_Z]);
1403 t[3] = (a[SG_Y] + a[SG_Z]) * (b[SG_X] - b[SG_W]);
1404 t[4] = (a[SG_X] + a[SG_Z]) * (b[SG_X] + b[SG_Y]);
1405 t[5] = (a[SG_X] - a[SG_Z]) * (b[SG_X] - b[SG_Y]);
1406 t[6] = (a[SG_W] + a[SG_Y]) * (b[SG_W] - b[SG_Z]);
1407 t[7] = (a[SG_W] - a[SG_Y]) * (b[SG_W] + b[SG_Z]);
1408
1409 dst[SG_W] = t[1] + ((-t[4] - t[5] + t[6] + t[7]) * SG_HALF);
1410 dst[SG_X] = t[0] - (( t[4] + t[5] + t[6] + t[7]) * SG_HALF);
1411 dst[SG_Y] = -t[2] + (( t[4] - t[5] + t[6] - t[7]) * SG_HALF);
1412 dst[SG_Z] = -t[3] + (( t[4] - t[5] - t[6] + t[7]) * SG_HALF);
1413 }
1414
1415 //from gamasutra.com
1416 //by nb@netcom.ca
1417
sgMultQuat2(sgQuat dst,const sgQuat a,const sgQuat b)1418 void sgMultQuat2 ( sgQuat dst, const sgQuat a, const sgQuat b )
1419 {
1420 SGfloat A, B, C, D, E, F, G, H;
1421
1422 A = (a[SG_W] + a[SG_X]) * (b[SG_W] + b[SG_X]) ;
1423 B = (a[SG_Z] - a[SG_Y]) * (b[SG_Y] - b[SG_Z]) ;
1424 C = (a[SG_X] - a[SG_W]) * (b[SG_Y] + b[SG_Z]) ;
1425 D = (a[SG_Y] + a[SG_Z]) * (b[SG_X] - b[SG_W]) ;
1426 E = (a[SG_X] + a[SG_Z]) * (b[SG_X] + b[SG_Y]) ;
1427 F = (a[SG_X] - a[SG_Z]) * (b[SG_X] - b[SG_Y]) ;
1428 G = (a[SG_W] + a[SG_Y]) * (b[SG_W] - b[SG_Z]) ;
1429 H = (a[SG_W] - a[SG_Y]) * (b[SG_W] + b[SG_Z]) ;
1430
1431
1432 dst[SG_W] = B + (-E - F + G + H) / SG_TWO ;
1433 dst[SG_X] = A - ( E + F + G + H) / SG_TWO ;
1434 dst[SG_Y] = -C + ( E - F + G - H) / SG_TWO ;
1435 dst[SG_Z] = -D + ( E - F - G + H) / SG_TWO ;
1436 }
1437
1438 //from gamasutra.com
1439 //by nb@netcom.ca
1440
sgEulerToQuat(sgQuat quat,const sgVec3 hpr)1441 void sgEulerToQuat(sgQuat quat, const sgVec3 hpr )
1442 {
1443 SGfloat cr, cp, cy, sr, sp, sy, cpcy, spsy;
1444
1445 // calculate trig identities
1446 cr = (SGfloat) cos(hpr[2]*SG_DEGREES_TO_RADIANS/SG_TWO);
1447 cp = (SGfloat) cos(hpr[1]*SG_DEGREES_TO_RADIANS/SG_TWO);
1448 cy = (SGfloat) cos(hpr[0]*SG_DEGREES_TO_RADIANS/SG_TWO);
1449
1450 sr = (SGfloat) sin(hpr[2]*SG_DEGREES_TO_RADIANS/SG_TWO);
1451 sp = (SGfloat) sin(hpr[1]*SG_DEGREES_TO_RADIANS/SG_TWO);
1452 sy = (SGfloat) sin(hpr[0]*SG_DEGREES_TO_RADIANS/SG_TWO);
1453
1454 cpcy = cp * cy;
1455 spsy = sp * sy;
1456
1457 quat[SG_W] = cr * cpcy + sr * spsy;
1458 quat[SG_X] = sr * cpcy - cr * spsy;
1459 quat[SG_Y] = cr * sp * cy + sr * cp * sy;
1460 quat[SG_Z] = cr * cp * sy - sr * sp * cy;
1461 }
1462
1463 //from darwin3d.com
1464 // jeffl@darwin3d.com
1465
sgQuatToEuler(sgVec3 hpr,const sgQuat quat)1466 void sgQuatToEuler( sgVec3 hpr, const sgQuat quat )
1467 {
1468 SGfloat matrix[3][3];
1469 SGfloat cx,sx;
1470 SGfloat cy,sy;
1471 SGfloat cz,sz;
1472
1473 // CONVERT QUATERNION TO MATRIX - I DON'T REALLY NEED ALL OF IT
1474
1475 matrix[0][0] = SG_ONE - (SG_TWO * quat[SG_Y] * quat[SG_Y])
1476 - (SG_TWO * quat[SG_Z] * quat[SG_Z]);
1477 //matrix[0][1] = (SG_TWO * quat->x * quat->y) - (SG_TWO * quat->w * quat->z);
1478 //matrix[0][2] = (SG_TWO * quat->x * quat->z) + (SG_TWO * quat->w * quat->y);
1479
1480 matrix[1][0] = (SG_TWO * quat[SG_X] * quat[SG_Y]) +
1481 (SG_TWO * quat[SG_W] * quat[SG_Z]);
1482 //matrix[1][1] = SG_ONE - (SG_TWO * quat->x * quat->x)
1483 // - (SG_TWO * quat->z * quat->z);
1484 //matrix[1][2] = (SG_TWO * quat->y * quat->z) - (SG_TWO * quat->w * quat->x);
1485
1486 matrix[2][0] = (SG_TWO * quat[SG_X] * quat[SG_Z]) -
1487 (SG_TWO * quat[SG_W] * quat[SG_Y]);
1488 matrix[2][1] = (SG_TWO * quat[SG_Y] * quat[SG_Z]) +
1489 (SG_TWO * quat[SG_W] * quat[SG_X]);
1490 matrix[2][2] = SG_ONE - (SG_TWO * quat[SG_X] * quat[SG_X])
1491 - (SG_TWO * quat[SG_Y] * quat[SG_Y]);
1492
1493 sy = -matrix[2][0];
1494 cy = (SGfloat)sgSqrt(SG_ONE - (sy * sy));
1495
1496 hpr[1] = sgATan2 ( sy, cy ) ;
1497
1498 // AVOID DIVIDE BY ZERO ERROR ONLY WHERE Y= +-90 or +-270
1499 // NOT CHECKING cy BECAUSE OF PRECISION ERRORS
1500 if (sy != SG_ONE && sy != -SG_ONE)
1501 {
1502 cx = matrix[2][2] / cy;
1503 sx = matrix[2][1] / cy;
1504 hpr[0] = sgATan2 ( sx, cx ) ;
1505
1506 cz = matrix[0][0] / cy;
1507 sz = matrix[1][0] / cy;
1508 hpr[2] = sgATan2 ( sz, cz ) ;
1509 }
1510 else
1511 {
1512 // SINCE Cos(Y) IS 0, I AM SCREWED. ADOPT THE STANDARD Z = 0
1513 // I THINK THERE IS A WAY TO FIX THIS BUT I AM NOT SURE. EULERS SUCK
1514 // NEED SOME MORE OF THE MATRIX TERMS NOW
1515
1516 matrix[1][1] = SG_ONE - (SG_TWO * quat[SG_X] * quat[SG_X])
1517 - (SG_TWO * quat[SG_Z] * quat[SG_Z]);
1518 matrix[1][2] = (SG_TWO * quat[SG_Y] * quat[SG_Z]) -
1519 (SG_TWO * quat[SG_W] * quat[SG_X]);
1520
1521 cx = matrix[1][1];
1522 sx = -matrix[1][2];
1523 hpr[0] = sgATan2 ( sx, cx ) ;
1524
1525 cz = SG_ONE ;
1526 sz = SG_ZERO ;
1527 hpr[2] = sgATan2 ( sz, cz ) ;
1528 }
1529 }
1530
1531
sgQuatToMatrix(sgMat4 dst,const sgQuat q)1532 void sgQuatToMatrix ( sgMat4 dst, const sgQuat q )
1533 {
1534 SGfloat two_xx = q[SG_X] * (q[SG_X] + q[SG_X]) ;
1535 SGfloat two_xy = q[SG_X] * (q[SG_Y] + q[SG_Y]) ;
1536 SGfloat two_xz = q[SG_X] * (q[SG_Z] + q[SG_Z]) ;
1537
1538 SGfloat two_wx = q[SG_W] * (q[SG_X] + q[SG_X]) ;
1539 SGfloat two_wy = q[SG_W] * (q[SG_Y] + q[SG_Y]) ;
1540 SGfloat two_wz = q[SG_W] * (q[SG_Z] + q[SG_Z]) ;
1541
1542 SGfloat two_yy = q[SG_Y] * (q[SG_Y] + q[SG_Y]) ;
1543 SGfloat two_yz = q[SG_Y] * (q[SG_Z] + q[SG_Z]) ;
1544
1545 SGfloat two_zz = q[SG_Z] * (q[SG_Z] + q[SG_Z]) ;
1546
1547 sgSetVec4 ( dst[0], SG_ONE-(two_yy+two_zz), two_xy-two_wz, two_xz+two_wy, SG_ZERO ) ;
1548 sgSetVec4 ( dst[1], two_xy+two_wz, SG_ONE-(two_xx+two_zz), two_yz-two_wx, SG_ZERO ) ;
1549 sgSetVec4 ( dst[2], two_xz-two_wy, two_yz+two_wx, SG_ONE-(two_xx+two_yy), SG_ZERO ) ;
1550 sgSetVec4 ( dst[3], SG_ZERO, SG_ZERO, SG_ZERO, SG_ONE ) ;
1551 }
1552
1553
1554 //from gamasutra.com
1555 //by nb@netcom.ca
1556
1557 /*****************************
1558 DEPRECATED...use sgQuatToMatrix instead.
1559 *****************************/
1560
sgMakeRotMat42(sgMat4 m,sgQuat quat)1561 void sgMakeRotMat42( sgMat4 m, sgQuat quat ){
1562 SGfloat wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
1563
1564 // calculate coefficients
1565 x2 = quat[SG_X] + quat[SG_X]; y2 = quat[SG_Y] + quat[SG_Y];
1566 z2 = quat[SG_Z] + quat[SG_Z];
1567 xx = quat[SG_X] * x2; xy = quat[SG_X] * y2; xz = quat[SG_X] * z2;
1568 yy = quat[SG_Y] * y2; yz = quat[SG_Y] * z2; zz = quat[SG_Z] * z2;
1569 wx = quat[SG_W] * x2; wy = quat[SG_W] * y2; wz = quat[SG_W] * z2;
1570
1571 m[0][0] = SG_ONE- (yy + zz); m[0][1] = xy - wz;
1572 m[0][2] = xz + wy; m[0][3] = SG_ZERO ;
1573
1574 m[1][0] = xy + wz; m[1][1] = SG_ONE- (xx + zz);
1575 m[1][2] = yz - wx; m[1][3] = SG_ZERO ;
1576
1577 m[2][0] = xz - wy; m[2][1] = yz + wx;
1578 m[2][2] = SG_ONE- (xx + yy); m[2][3] = SG_ZERO ;
1579
1580 m[3][0] = 0; m[3][1] = 0;
1581 m[3][2] = 0; m[3][3] = 1;
1582 }
1583
1584 //from gamasutra.com
1585 //by nb@netcom.ca
1586
sgSlerpQuat2(sgQuat dst,const sgQuat from,const sgQuat to,const SGfloat t)1587 void sgSlerpQuat2( sgQuat dst, const sgQuat from, const sgQuat to, const SGfloat t )
1588 {
1589 SGfloat to1[4];
1590 SGfloat omega, cosom, sinom, scale0, scale1;
1591
1592 // calc cosine
1593 cosom = from[SG_X] * to[SG_X] +
1594 from[SG_Y] * to[SG_Y] +
1595 from[SG_Z] * to[SG_Z] +
1596 from[SG_W] * to[SG_W];
1597
1598 // adjust signs (if necessary)
1599
1600 if ( cosom < SG_ZERO )
1601 {
1602 cosom = -cosom;
1603 to1[0] = - to[SG_X];
1604 to1[1] = - to[SG_Y];
1605 to1[2] = - to[SG_Z];
1606 to1[3] = - to[SG_W];
1607 }
1608 else
1609 {
1610 to1[0] = to[SG_X];
1611 to1[1] = to[SG_Y];
1612 to1[2] = to[SG_Z];
1613 to1[3] = to[SG_W];
1614 }
1615
1616 // calculate coefficients
1617 #define DELTA SG_ZERO
1618 if ( (SG_ONE- cosom) > DELTA )
1619 {
1620 // standard case (slerp)
1621 omega = (SGfloat) acos(cosom);
1622 sinom = (SGfloat) sin(omega);
1623 scale0 = (SGfloat) sin((SG_ONE- t) * omega) / sinom;
1624 scale1 = (SGfloat) sin(t * omega) / sinom;
1625 }
1626 else
1627 {
1628 // "from" and "to" quaternions are very close
1629 // ... so we can do a linear interpolation
1630 scale0 = SG_ONE- t;
1631 scale1 = t;
1632 }
1633
1634 // calculate final values
1635 dst[SG_X] = scale0 * from[SG_X] + scale1 * to1[0];
1636 dst[SG_Y] = scale0 * from[SG_Y] + scale1 * to1[1];
1637 dst[SG_Z] = scale0 * from[SG_Z] + scale1 * to1[2];
1638 dst[SG_W] = scale0 * from[SG_W] + scale1 * to1[3];
1639 }
1640
sgSlerpQuat(sgQuat dst,const sgQuat from,const sgQuat to,const SGfloat t)1641 void sgSlerpQuat( sgQuat dst, const sgQuat from, const sgQuat to, const SGfloat t )
1642 {
1643 SGfloat co, scale0, scale1;
1644 bool flip = false ;
1645
1646 /* SWC - Interpolate between to quaternions */
1647
1648 co = sgScalarProductVec4 ( from, to ) ;
1649
1650 if ( co < SG_ZERO )
1651 {
1652 co = -co;
1653 flip = true ;
1654 }
1655
1656 if ( co < SG_ONE - (SGfloat) 1e-6 )
1657 {
1658 SGfloat o = (SGfloat) acos ( co );
1659 SGfloat so = SG_ONE / (SGfloat) sin ( o );
1660
1661 scale0 = (SGfloat) sin ( (SG_ONE - t) * o ) * so;
1662 scale1 = (SGfloat) sin ( t * o ) * so;
1663 }
1664 else
1665 {
1666 scale0 = SG_ONE - t;
1667 scale1 = t;
1668 }
1669
1670 if ( flip )
1671 {
1672 scale1 = -scale1 ;
1673 }
1674
1675 dst[SG_X] = scale0 * from[SG_X] + scale1 * to[SG_X] ;
1676 dst[SG_Y] = scale0 * from[SG_Y] + scale1 * to[SG_Y] ;
1677 dst[SG_Z] = scale0 * from[SG_Z] + scale1 * to[SG_Z] ;
1678 dst[SG_W] = scale0 * from[SG_W] + scale1 * to[SG_W] ;
1679 }
1680
1681
1682
1683 /* Function to rotate a vector through a given quaternion using the formula
1684 * R = Q r Q-1 -- this gives the components of a ROTATED vector in a STATIONARY
1685 * coordinate system. We assume that Q is a unit quaternion.
1686 */
sgRotateVecQuat(sgVec3 vec,sgQuat q)1687 void sgRotateVecQuat ( sgVec3 vec, sgQuat q )
1688 {
1689 sgVec3 rot ;
1690 sgFloat qwqw = q[SG_W] * q[SG_W] ;
1691 sgFloat qwqx = q[SG_W] * q[SG_X] ;
1692 sgFloat qwqy = q[SG_W] * q[SG_Y] ;
1693 sgFloat qwqz = q[SG_W] * q[SG_Z] ;
1694 sgFloat qxqx = q[SG_X] * q[SG_X] ;
1695 sgFloat qxqy = q[SG_X] * q[SG_Y] ;
1696 sgFloat qxqz = q[SG_X] * q[SG_Z] ;
1697 sgFloat qyqy = q[SG_Y] * q[SG_Y] ;
1698 sgFloat qyqz = q[SG_Y] * q[SG_Z] ;
1699 sgFloat qzqz = q[SG_Z] * q[SG_Z] ;
1700 rot[SG_X] = ( qwqw + qxqx - qyqy - qzqz ) * vec[SG_X] + 2.0f * ( qxqy - qwqz ) * vec[SG_Y] + 2.0f * ( qxqz + qwqy ) * vec[SG_Z] ;
1701 rot[SG_Y] = ( qwqw - qxqx + qyqy - qzqz ) * vec[SG_Y] + 2.0f * ( qyqz - qwqx ) * vec[SG_Z] + 2.0f * ( qxqy + qwqz ) * vec[SG_X] ;
1702 rot[SG_Z] = ( qwqw - qxqx - qyqy + qzqz ) * vec[SG_Z] + 2.0f * ( qxqz - qwqy ) * vec[SG_X] + 2.0f * ( qyqz + qwqx ) * vec[SG_Y] ;
1703 sgCopyVec3 ( vec, rot ) ;
1704 }
1705
1706 /* Function to rotate a vector through a given quaternion using the formula
1707 * R = Q-1 r Q -- this gives the components of a STATIONARY vector in a ROTATED
1708 * coordinate system. We assume that Q is a unit quaternion.
1709 */
sgRotateCoordQuat(sgVec3 vec,sgQuat q)1710 void sgRotateCoordQuat ( sgVec3 vec, sgQuat q )
1711 {
1712 sgVec3 rot ;
1713 sgFloat qwqw = q[SG_W] * q[SG_W] ;
1714 sgFloat qwqx = q[SG_W] * q[SG_X] ;
1715 sgFloat qwqy = q[SG_W] * q[SG_Y] ;
1716 sgFloat qwqz = q[SG_W] * q[SG_Z] ;
1717 sgFloat qxqx = q[SG_X] * q[SG_X] ;
1718 sgFloat qxqy = q[SG_X] * q[SG_Y] ;
1719 sgFloat qxqz = q[SG_X] * q[SG_Z] ;
1720 sgFloat qyqy = q[SG_Y] * q[SG_Y] ;
1721 sgFloat qyqz = q[SG_Y] * q[SG_Z] ;
1722 sgFloat qzqz = q[SG_Z] * q[SG_Z] ;
1723 rot[SG_X] = ( qwqw + qxqx - qyqy - qzqz ) * vec[SG_X] + 2.0f * ( qxqy + qwqz ) * vec[SG_Y] + 2.0f * ( qxqz - qwqy ) * vec[SG_Z] ;
1724 rot[SG_Y] = ( qwqw - qxqx + qyqy - qzqz ) * vec[SG_Y] + 2.0f * ( qyqz + qwqx ) * vec[SG_Z] + 2.0f * ( qxqy - qwqz ) * vec[SG_X] ;
1725 rot[SG_Z] = ( qwqw - qxqx - qyqy + qzqz ) * vec[SG_Z] + 2.0f * ( qxqz + qwqy ) * vec[SG_X] + 2.0f * ( qyqz - qwqx ) * vec[SG_Y] ;
1726 sgCopyVec3 ( vec, rot ) ;
1727 }
1728
1729
sgDistSquaredToLineLineSegment(const sgLineSegment3 seg,const sgLine3 line)1730 sgFloat sgDistSquaredToLineLineSegment ( const sgLineSegment3 seg, const sgLine3 line )
1731 {
1732 /* Convert the line segment to a line. We will deal with the segment limits later. */
1733 sgLine3 line2 ;
1734 sgLineSegment3ToLine3 ( &line2, seg ) ;
1735
1736 /* Get the dot product of the two direction vectors */
1737 sgFloat t1_dot_t2 = sgScalarProductVec3 ( line.direction_vector, line2.direction_vector ) ;
1738
1739 /* If the lines are parallel, distance is just the distance from a point to the other line */
1740 if ( fabs ( t1_dot_t2 ) >= 1.0 )
1741 return sgDistSquaredToLineVec3 ( line, seg.a ) ;
1742
1743 /* Get the parametric coordinates of the closest points on the two lines. The first line
1744 * is parameterized by r = r1 + t1 u while the second is parameterized by r = r2 + t2 v.
1745 * The square of the distance between them is [ ( r1 + t1 u ) - ( r2 + t2 v ) ] dot
1746 * [ ( r1 + t1 u ) - ( r2 + t2 v ) ]. Differentiating this dot product with respect to
1747 * u and v and setting the derivatives to zero gives a matrix equation:
1748 * [ 1 -(t1 dot t2) ] [ u ] = [ ( r1 - r2 ) dot t1 ]
1749 * [ -(t1 dot t2) 1 ] [ v ] [ -( r1 - r2 ) dot t2 ]
1750 * We invert the matrix to get the equations below.
1751 */
1752 sgVec3 r1_minus_r2 ;
1753 sgSubVec3 ( r1_minus_r2, line.point_on_line, line2.point_on_line ) ;
1754
1755 /* t1_t2_t2_minus_t1 = ( t1 dot t2 ) t2 - t1
1756 * t2_minus_t1_t2_t1 = t2 - ( t1 dot t2 ) t1
1757 */
1758 sgVec3 t1_t2_t2_minus_t1, t2_minus_t1_t2_t1 ;
1759 sgAddScaledVec3 ( t1_t2_t2_minus_t1, line.direction_vector, line2.direction_vector, -t1_dot_t2 ) ;
1760 sgNegateVec3 ( t1_t2_t2_minus_t1 ) ;
1761 sgAddScaledVec3 ( t2_minus_t1_t2_t1, line2.direction_vector, line.direction_vector, -t1_dot_t2 ) ;
1762
1763 sgFloat u = sgScalarProductVec3 ( r1_minus_r2, t1_t2_t2_minus_t1 ) / ( 1.0f - t1_dot_t2 * t1_dot_t2 ) ;
1764 sgFloat v = sgScalarProductVec3 ( r1_minus_r2, t2_minus_t1_t2_t1 ) / ( 1.0f - t1_dot_t2 * t1_dot_t2 ) ;
1765
1766 /* Since line 2 is a line segment, we limit "v" to between 0 and the distance between the points. */
1767 sgFloat length = sgDistanceVec3 ( seg.a, seg.b ) ;
1768 if ( v < 0.0 ) v = 0.0 ;
1769 if ( v > length ) v = length ;
1770
1771 sgVec3 point1, point2 ;
1772 sgAddScaledVec3 ( point1, line.point_on_line, line.direction_vector, u ) ;
1773 sgAddScaledVec3 ( point2, line2.point_on_line, line2.direction_vector, v ) ;
1774 return sgDistanceSquaredVec3 ( point1, point2 ) ;
1775 }
1776
1777
sgReflectInPlaneVec3(sgVec3 dst,const sgVec3 src,const sgVec4 plane)1778 void sgReflectInPlaneVec3 ( sgVec3 dst, const sgVec3 src, const sgVec4 plane )
1779 {
1780 SGfloat src_dot_norm = sgScalarProductVec3 ( src, plane ) ;
1781
1782 sgVec3 tmp ;
1783
1784 sgScaleVec3 ( tmp, plane, SG_TWO * src_dot_norm ) ;
1785 sgSubVec3 ( dst, src, tmp ) ;
1786 }
1787
1788
1789
sgClassifyMat4(const sgMat4 m)1790 int sgClassifyMat4 ( const sgMat4 m )
1791 {
1792 const SGfloat epsilon = 1e-6f ;
1793
1794 int flags = 0 ;
1795
1796
1797 SGfloat sx, sy, sz ;
1798
1799 if ( m[0][1] == SG_ZERO && m[0][2] == SG_ZERO &&
1800 m[1][0] == SG_ZERO && m[1][2] == SG_ZERO &&
1801 m[2][0] == SG_ZERO && m[2][1] == SG_ZERO )
1802 {
1803
1804 int n = ( m[0][0] < 0 ) + ( m[1][1] < 0 ) + ( m[2][2] < 0 ) ;
1805
1806 if ( n > 1 )
1807 flags |= SG_ROTATION ;
1808
1809 if ( n % 2 != 0 )
1810 flags |= SG_MIRROR ;
1811
1812 sx = m[0][0] * m[0][0] ;
1813 sy = m[1][1] * m[1][1] ;
1814 sz = m[2][2] * m[2][2] ;
1815
1816 }
1817 else
1818 {
1819
1820 flags |= SG_ROTATION ;
1821
1822 if ( sgAbs ( sgScalarProductVec3 ( m[1], m[2] ) ) > epsilon ||
1823 sgAbs ( sgScalarProductVec3 ( m[2], m[0] ) ) > epsilon ||
1824 sgAbs ( sgScalarProductVec3 ( m[0], m[1] ) ) > epsilon )
1825 {
1826 flags |= SG_NONORTHO ;
1827 }
1828
1829 sgVec3 temp ;
1830 sgVectorProductVec3 ( temp, m[0], m[1] ) ;
1831 SGfloat det = sgScalarProductVec3 ( temp, m[2] ) ;
1832
1833 if ( det < 0 )
1834 flags |= SG_MIRROR ;
1835
1836 sx = sgScalarProductVec3 ( m[0], m[0] ) ;
1837 sy = sgScalarProductVec3 ( m[1], m[1] ) ;
1838 sz = sgScalarProductVec3 ( m[2], m[2] ) ;
1839
1840 }
1841
1842
1843 if ( sgAbs ( sx - sy ) > epsilon ||
1844 sgAbs ( sx - sz ) > epsilon )
1845 {
1846 flags |= SG_NONORTHO ;
1847 flags |= SG_GENERAL_SCALE ; // also set general scale bit, though it may be deleted in the future
1848 }
1849 else
1850 {
1851 if ( sgAbs ( sx - SG_ONE ) > epsilon )
1852 flags |= SG_SCALE ;
1853 }
1854
1855
1856 if ( m[3][0] != SG_ZERO || m[3][1] != SG_ZERO || m[3][2] != SG_ZERO )
1857 {
1858 flags |= SG_TRANSLATION ;
1859 }
1860
1861
1862 if ( m[0][3] != SG_ZERO || m[1][3] != SG_ZERO || m[2][3] != SG_ZERO ||
1863 m[3][3] != SG_ONE )
1864 {
1865 flags |= SG_PROJECTION ;
1866 }
1867
1868
1869 return flags ;
1870 }
1871
1872
sgTriangleSolver_ASAtoArea(SGfloat angA,SGfloat lenB,SGfloat angC)1873 SGfloat sgTriangleSolver_ASAtoArea ( SGfloat angA, SGfloat lenB, SGfloat angC )
1874 {
1875 /* Get the third angle */
1876
1877 SGfloat angB = SG_180 - (angA + angC) ;
1878
1879 /* Use Sine Rule to get length of a second side - then use SAStoArea. */
1880
1881 SGfloat sinB = sgSin ( angB ) ;
1882
1883 if ( sinB == SG_ZERO )
1884 return SG_ZERO ;
1885
1886 SGfloat lenA = lenB * sgSin(angA) / sinB ;
1887
1888 return sgTriangleSolver_SAStoArea ( lenA, angC, lenB ) ;
1889 }
1890
1891
sgTriangleSolver_SAStoArea(SGfloat lenA,SGfloat angB,SGfloat lenC)1892 SGfloat sgTriangleSolver_SAStoArea ( SGfloat lenA, SGfloat angB, SGfloat lenC )
1893 {
1894 return SG_HALF * lenC * lenA * sgSin ( angB ) ;
1895 }
1896
1897
sgTriangleSolver_SSStoArea(SGfloat lenA,SGfloat lenB,SGfloat lenC)1898 SGfloat sgTriangleSolver_SSStoArea ( SGfloat lenA, SGfloat lenB, SGfloat lenC )
1899 {
1900 /* Heron's formula */
1901
1902 SGfloat s = ( lenA + lenB + lenC ) / SG_TWO ;
1903 SGfloat q = s * (s-lenA) * (s-lenB) * (s-lenC) ;
1904
1905 /* Ikky illegal triangles generate zero areas. */
1906
1907 return ( q <= SG_ZERO ) ? SG_ZERO : sgSqrt ( q ) ;
1908 }
1909
1910
sgTriangleSolver_ASStoArea(SGfloat angB,SGfloat lenA,SGfloat lenB,int angA_is_obtuse)1911 SGfloat sgTriangleSolver_ASStoArea ( SGfloat angB, SGfloat lenA, SGfloat lenB,
1912 int angA_is_obtuse )
1913 {
1914 SGfloat lenC ;
1915
1916 sgTriangleSolver_ASStoSAA ( angB, lenA, lenB, angA_is_obtuse,
1917 &lenC, NULL, NULL ) ;
1918
1919 return sgTriangleSolver_SAStoArea ( lenA, angB, lenC ) ;
1920 }
1921
sgTriangleSolver_SAAtoArea(SGfloat lenA,SGfloat angB,SGfloat angA)1922 SGfloat sgTriangleSolver_SAAtoArea ( SGfloat lenA, SGfloat angB, SGfloat angA )
1923 {
1924 SGfloat lenC ;
1925
1926 sgTriangleSolver_SAAtoASS ( lenA, angB, angA, NULL, NULL, &lenC ) ;
1927
1928 return sgTriangleSolver_SAStoArea ( lenA, angB, lenC ) ;
1929 }
1930
sgTriangleSolver_SSStoAAA(SGfloat lenA,SGfloat lenB,SGfloat lenC,SGfloat * angA,SGfloat * angB,SGfloat * angC)1931 void sgTriangleSolver_SSStoAAA ( SGfloat lenA, SGfloat lenB, SGfloat lenC,
1932 SGfloat *angA, SGfloat *angB, SGfloat *angC )
1933 {
1934 SGfloat aa, bb, cc ;
1935
1936 int flag = ( lenA == SG_ZERO ) |
1937 (( lenB == SG_ZERO )<<1) |
1938 (( lenC == SG_ZERO )<<2) ;
1939
1940 /* Ikky zero-sized triangles generate zero/90 angles appropriately. */
1941 /* Ikky triangles with all lengths zero generate 60 degree angles. */
1942 /* Ikky impossible triangles generate all zero angles. */
1943
1944 switch ( flag )
1945 {
1946 case 0 : /* no zero-lengthed sides */
1947 /* Cosine law */
1948 aa = sgACos (( lenB*lenB + lenC*lenC - lenA*lenA )/(SG_TWO*lenB*lenC)) ;
1949 bb = sgACos (( lenA*lenA + lenC*lenC - lenB*lenB )/(SG_TWO*lenA*lenC)) ;
1950 cc = sgACos (( lenA*lenA + lenB*lenB - lenC*lenC )/(SG_TWO*lenA*lenB)) ;
1951 break ;
1952
1953 case 1 : /* lenA is zero */
1954 aa = SG_ZERO ;
1955 bb = cc = SG_90 ;
1956 break ;
1957
1958 case 2 : /* lenB is zero */
1959 bb = SG_ZERO ;
1960 aa = cc = SG_90 ;
1961 break ;
1962
1963 case 4 : /* lenC is zero */
1964 cc = SG_ZERO ;
1965 aa = bb = SG_90 ;
1966 break ;
1967
1968 case 3 : /* Two lengths are zero and the third isn't?!? */
1969 case 5 :
1970 case 6 :
1971 aa = bb = cc = SG_ZERO ;
1972 break ;
1973
1974 default : /* All three sides are zero length */
1975 aa = bb = cc = SG_60 ;
1976 break ;
1977 }
1978
1979 if ( angA ) *angA = aa ;
1980 if ( angB ) *angB = bb ;
1981 if ( angC ) *angC = cc ;
1982 }
1983
sgTriangleSolver_SAStoASA(SGfloat lenA,SGfloat angB,SGfloat lenC,SGfloat * angA,SGfloat * lenB,SGfloat * angC)1984 void sgTriangleSolver_SAStoASA ( SGfloat lenA, SGfloat angB, SGfloat lenC,
1985 SGfloat *angA, SGfloat *lenB, SGfloat *angC )
1986 {
1987 /* Get third side using Cosine Rule */
1988
1989 SGfloat s = lenC * lenC +
1990 lenA * lenA - SG_TWO * lenC * lenA * sgCos( angB ) ;
1991
1992 SGfloat lb = ( s <= SG_ZERO ) ? SG_ZERO : (SGfloat) sqrt ( s ) ;
1993
1994 if ( lenB ) *lenB = lb ;
1995
1996 sgTriangleSolver_SSStoAAA ( lenA, lb, lenC, angA, NULL, angC ) ;
1997 }
1998
1999
sgTriangleSolver_ASAtoSAS(SGfloat angA,SGfloat lenB,SGfloat angC,SGfloat * lenA,SGfloat * angB,SGfloat * lenC)2000 void sgTriangleSolver_ASAtoSAS ( SGfloat angA, SGfloat lenB, SGfloat angC,
2001 SGfloat *lenA, SGfloat *angB, SGfloat *lenC )
2002 {
2003 /* Find the missing angle */
2004
2005 SGfloat bb = SG_180 - (angA + angC) ;
2006
2007 if ( angB ) *angB = bb ;
2008
2009 /* Use Sine Rule */
2010
2011 SGfloat sinB = sgSin ( bb ) ;
2012
2013 if ( sinB == SG_ZERO )
2014 {
2015 if ( lenA ) *lenA = lenB / SG_TWO ; /* One valid interpretation */
2016 if ( lenC ) *lenC = lenB / SG_TWO ;
2017 }
2018 else
2019 {
2020 if ( lenA ) *lenA = lenB * sgSin(angA) / sinB ;
2021 if ( lenC ) *lenC = lenB * sgSin(angC) / sinB ;
2022 }
2023 }
2024
2025
sgTriangleSolver_ASStoSAA(SGfloat angB,SGfloat lenA,SGfloat lenB,int angA_is_obtuse,SGfloat * lenC,SGfloat * angA,SGfloat * angC)2026 void sgTriangleSolver_ASStoSAA ( SGfloat angB, SGfloat lenA, SGfloat lenB,
2027 int angA_is_obtuse,
2028 SGfloat *lenC, SGfloat *angA, SGfloat *angC )
2029 {
2030 /* Sine law */
2031
2032 SGfloat aa = (lenB == SG_ZERO ) ? SG_ZERO : sgASin (lenA * sgSin(angB)/lenB) ;
2033
2034 if ( angA_is_obtuse )
2035 aa = SG_180 - aa ;
2036
2037 if ( angA ) *angA = aa ;
2038
2039 /* Find the missing angle */
2040
2041 SGfloat cc = SG_180 - (aa + angB) ;
2042
2043 if ( angC ) *angC = cc ;
2044
2045 /* Use SAStoASA to get the last length */
2046
2047 sgTriangleSolver_SAStoASA ( lenA, cc, lenB, NULL, lenC, NULL ) ;
2048 }
2049
2050
sgTriangleSolver_SAAtoASS(SGfloat lenA,SGfloat angB,SGfloat angA,SGfloat * angC,SGfloat * lenB,SGfloat * lenC)2051 void sgTriangleSolver_SAAtoASS ( SGfloat lenA, SGfloat angB, SGfloat angA,
2052 SGfloat *angC, SGfloat *lenB, SGfloat *lenC )
2053 {
2054 /* Find the missing angle */
2055
2056 SGfloat cc = SG_180 - (angB + angA) ;
2057
2058 if ( angC ) *angC = cc ;
2059
2060 sgTriangleSolver_ASAtoSAS ( cc, lenA, angB, lenC, NULL, lenB ) ;
2061 }
2062
2063
2064