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: sgd.cxx 1944 2004-08-05 01:07:09Z puggles $
22 */
23
24
25 #include "sg.h"
26
27 sgdVec3 _sgdGravity = { 0.0f, 0.0f, -9.8f } ;
28
sgdVectorProductVec3(sgdVec3 dst,const sgdVec3 a,const sgdVec3 b)29 void sgdVectorProductVec3 ( sgdVec3 dst, const sgdVec3 a, const sgdVec3 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
_sgdClampToUnity(const SGDfloat x)36 inline SGDfloat _sgdClampToUnity ( const SGDfloat x )
37 {
38 if ( x > SGD_ONE ) return SGD_ONE ;
39 if ( x < -SGD_ONE ) return -SGD_ONE ;
40 return x ;
41 }
42
sgdCompare3DSqdDist(const sgdVec3 v1,const sgdVec3 v2,const SGDfloat sqd_dist)43 int sgdCompare3DSqdDist( const sgdVec3 v1, const sgdVec3 v2, const SGDfloat sqd_dist )
44 {
45 sgdVec3 tmp ;
46
47 sgdSubVec3 ( tmp, v2, v1 ) ;
48
49 SGDfloat 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
sgdMakeRotMat4(sgdMat4 mat,const SGDfloat angle,const sgdVec3 axis)56 void sgdMakeRotMat4( sgdMat4 mat, const SGDfloat angle, const sgdVec3 axis )
57 {
58 sgdVec3 ax ;
59 sgdNormalizeVec3 ( ax, axis ) ;
60
61 SGDfloat temp_angle = angle * SGD_DEGREES_TO_RADIANS ;
62 SGDfloat s = sin ( temp_angle ) ;
63 SGDfloat c = cos ( temp_angle ) ;
64 SGDfloat t = SGD_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] = SGD_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] = SGD_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] = SGD_ZERO ;
80
81 mat[3][0] = SGD_ZERO ;
82 mat[3][1] = SGD_ZERO ;
83 mat[3][2] = SGD_ZERO ;
84 mat[3][3] = SGD_ONE ;
85 }
86
87
88
89
sgdMakePickMatrix(sgdMat4 mat,sgdFloat x,sgdFloat y,sgdFloat width,sgdFloat height,sgdVec4 viewport)90 void sgdMakePickMatrix( sgdMat4 mat, sgdFloat x, sgdFloat y,
91 sgdFloat width, sgdFloat height, sgdVec4 viewport )
92 {
93 sgdFloat sx = viewport[2] / width ;
94 sgdFloat sy = viewport[3] / height ;
95 sgdFloat tx = ( viewport[2] + SGD_TWO * (viewport[0] - x) ) / width ;
96 sgdFloat ty = ( viewport[3] + SGD_TWO * (viewport[1] - y) ) / height ;
97
98 mat[0][0] = sx ;
99 mat[0][1] = SGD_ZERO ;
100 mat[0][2] = SGD_ZERO ;
101 mat[0][3] = SGD_ZERO ;
102
103 mat[1][0] = SGD_ZERO ;
104 mat[1][1] = sy ;
105 mat[1][2] = SGD_ZERO ;
106 mat[1][3] = SGD_ZERO ;
107
108 mat[2][0] = SGD_ZERO ;
109 mat[2][1] = SGD_ZERO ;
110 mat[2][2] = SGD_ONE ;
111 mat[2][3] = SGD_ZERO ;
112
113 mat[3][0] = tx ;
114 mat[3][1] = ty ;
115 mat[3][2] = SGD_ZERO ;
116 mat[3][3] = SGD_ONE ;
117 }
118
119
120
sgdMakeLookAtMat4(sgdMat4 dst,const sgdVec3 eye,const sgdVec3 center,const sgdVec3 up)121 void sgdMakeLookAtMat4 ( sgdMat4 dst, const sgdVec3 eye,
122 const sgdVec3 center,
123 const sgdVec3 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 sgdVec3 x,y,z;
133
134 /* Y vector = center - eye */
135 sgdSubVec3 ( y, center, eye ) ;
136
137 /* Z vector = up */
138 sgdCopyVec3 ( z, up ) ;
139
140 /* X vector = Y cross Z */
141 sgdVectorProductVec3 ( x, y, z ) ;
142
143 /* Recompute Z = X cross Y */
144 sgdVectorProductVec3 ( z, x, y ) ;
145
146 /* Normalize everything */
147 sgdNormaliseVec3 ( x ) ;
148 sgdNormaliseVec3 ( y ) ;
149 sgdNormaliseVec3 ( z ) ;
150
151 /* Build the matrix */
152 sgdSetVec4 ( dst[0], x[0], x[1], x[2], SGD_ZERO ) ;
153 sgdSetVec4 ( dst[1], y[0], y[1], y[2], SGD_ZERO ) ;
154 sgdSetVec4 ( dst[2], z[0], z[1], z[2], SGD_ZERO ) ;
155 sgdSetVec4 ( dst[3], eye[0], eye[1], eye[2], SGD_ONE ) ;
156 }
157
158 // -dw- inconsistent linkage!
159
sgdTriArea(sgdVec3 p0,sgdVec3 p1,sgdVec3 p2)160 sgdFloat sgdTriArea( sgdVec3 p0, sgdVec3 p1, sgdVec3 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 sgdTriArea( int nsides, float **vv )
170 and changing the normal calculation and the for loop appropriately
171 sgdMakeNormal( norm, vv[0], vv[1], vv[2] )
172 for( int i=0; i<n; i++ )
173 */
174
175 sgdVec3 sum;
176 sgdZeroVec3( sum );
177
178 sgdVec3 norm;
179 sgdMakeNormal( norm, p0, p1, p2 );
180
181 sgdFloat *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 sgdFloat area = sgdAbs ( sgdScalarProductVec3 ( norm, sum ) ) ;
196
197 return area / 2.0f ;
198 }
199
200 /***************************************************\
201 * functions to get the angle between two vectors *
202 \***************************************************/
203
sgdAngleBetweenVec3(sgdVec3 v1,sgdVec3 v2)204 SGDfloat sgdAngleBetweenVec3 ( sgdVec3 v1, sgdVec3 v2 )
205 {
206 sgdVec3 nv1, nv2 ;
207
208 sgdNormalizeVec3 ( nv1, v1 ) ;
209 sgdNormalizeVec3 ( nv2, v2 ) ;
210 return sgdAngleBetweenNormalizedVec3 ( nv1, nv2 ) ;
211 }
212
sgdAngleBetweenNormalizedVec3(sgdVec3 first,sgdVec3 second,sgdVec3 normal)213 SGDfloat sgdAngleBetweenNormalizedVec3 (sgdVec3 first, sgdVec3 second, sgdVec3 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 SGDfloat myCos, abs1, abs2, SProduct, deltaAngle, myNorm;
222
223 if((normal[0]==0) && (normal[1]==0) && (normal[2]==0))
224 {
225 ulSetError ( UL_WARNING, "sgdGetAngleBetweenVectors: Normal is zero.");
226 return 0.0 ;
227 }
228
229 sgdVec3 temp;
230
231 sgdVectorProductVec3( temp, first, second);
232
233 myNorm = sgdLengthVec3 ( temp );
234
235 if ( (sgdScalarProductVec3(temp, normal))<0 )
236 myNorm = -myNorm;
237
238 if ( myNorm < -0.99999 )
239 deltaAngle = -SGD_PI*0.5;
240 else
241 if ( myNorm > 0.99999 )
242 deltaAngle = SGD_PI*0.5;
243 else
244 deltaAngle = (SGDfloat)asin((double)myNorm);
245
246 // deltaAngle is in the range -SGD_PI*0.5 to +SGD_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 sgdScalarProductVec3(first, second)
253
254 if ( deltaAngle < 0 )
255 deltaAngle = deltaAngle + 2*SGD_PI; // unnessecary?
256
257 SProduct = sgdScalarProductVec3(first, second);
258 myCos = (SGDfloat) 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 <= SGD_PI )
273 deltaAngle = SGD_PI - deltaAngle ;
274 else
275 deltaAngle = 3*SGD_PI - deltaAngle ;
276 }
277
278 assert ( deltaAngle >= 0.0 ) ;
279 assert ( deltaAngle <= 2.0*SGD_PI ) ;
280
281 return deltaAngle * SGD_RADIANS_TO_DEGREES ;
282 }
283
284
sgdAngleBetweenVec3(sgdVec3 v1,sgdVec3 v2,sgdVec3 normal)285 SGDfloat sgdAngleBetweenVec3 ( sgdVec3 v1, sgdVec3 v2, sgdVec3 normal )
286 {
287 // nornmal has to be normalized.
288 sgdVec3 nv1, nv2 ;
289
290 sgdNormalizeVec3 ( nv1, v1 ) ;
291 sgdNormalizeVec3 ( nv2, v2 ) ;
292 return sgdAngleBetweenNormalizedVec3 ( nv1, nv2, normal ) ;
293 }
294
295
296 /*********************\
297 * sgdBox routines *
298 \*********************/
299
300
extend(const sgdVec3 v)301 void sgdBox::extend ( const sgdVec3 v )
302 {
303 if ( isEmpty () )
304 {
305 sgdCopyVec3 ( min, v ) ;
306 sgdCopyVec3 ( 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 sgdBox * b)320 void sgdBox::extend ( const sgdBox *b )
321 {
322 if ( b -> isEmpty () )
323 return ;
324
325 if ( isEmpty () )
326 {
327 sgdCopyVec3 ( min, b->getMin() ) ;
328 sgdCopyVec3 ( max, b->getMax() ) ;
329 }
330 else
331 {
332 extend ( b->getMin() ) ;
333 extend ( b->getMax() ) ;
334 }
335 }
336
337
extend(const sgdSphere * s)338 void sgdBox::extend ( const sgdSphere *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 sgdVec3 x ;
349
350 sgdSetVec3 ( x, s->getCenter()[0]+s->getRadius(),
351 s->getCenter()[1]+s->getRadius(),
352 s->getCenter()[2]+s->getRadius() ) ;
353 extend ( x ) ;
354
355 sgdSetVec3 ( 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 sgdVec4 plane) const362 int sgdBox::intersects ( const sgdVec4 plane ) const
363 {
364 /*
365 Save multiplies by not redoing Ax+By+Cz+D for each point.
366 */
367
368 SGDfloat Ax_min = plane[0] * min[0] ;
369 SGDfloat By_min = plane[1] * min[1] ;
370 SGDfloat Cz_min_plus_D = plane[2] * min[2] + plane[3] ;
371
372 SGDfloat Ax_max = plane[0] * max[0] ;
373 SGDfloat By_max = plane[1] * max[1] ;
374 SGDfloat 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 > SGD_ZERO ) +
381 ( Ax_min + By_min + Cz_max_plus_D > SGD_ZERO ) +
382 ( Ax_min + By_max + Cz_min_plus_D > SGD_ZERO ) +
383 ( Ax_min + By_max + Cz_max_plus_D > SGD_ZERO ) +
384 ( Ax_max + By_min + Cz_min_plus_D > SGD_ZERO ) +
385 ( Ax_max + By_min + Cz_max_plus_D > SGD_ZERO ) +
386 ( Ax_max + By_max + Cz_min_plus_D > SGD_ZERO ) +
387 ( Ax_max + By_max + Cz_max_plus_D > SGD_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 * sgdSphere routines *
401 \**********************/
402
extend(const sgdVec3 v)403 void sgdSphere::extend ( const sgdVec3 v )
404 {
405 if ( isEmpty () )
406 {
407 sgdCopyVec3 ( center, v ) ;
408 radius = SGD_ZERO ;
409 return ;
410 }
411
412 SGDfloat d = sgdDistanceVec3 ( center, v ) ;
413
414 if ( d <= radius ) /* Point is already inside sphere */
415 return ;
416
417 SGDfloat new_radius = (radius + d) / SGD_TWO ; /* Grow radius */
418
419 SGDfloat 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 sgdBox * b)429 void sgdSphere::extend ( const sgdBox *b )
430 {
431 if ( b -> isEmpty () )
432 return ;
433
434 if ( isEmpty() )
435 {
436 sgdAddVec3 ( center, b->getMin(), b->getMax() ) ;
437 sgdScaleVec3 ( center, SGD_HALF ) ;
438 radius = sgdDistanceVec3 ( 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 sgdSphere 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 sgdVec3 x ;
472 extend ( b->getMin() ) ;
473 sgdSetVec3 ( x, b->getMin()[0],b->getMin()[1],b->getMax()[2] ) ; extend ( x ) ;
474 sgdSetVec3 ( x, b->getMin()[0],b->getMax()[1],b->getMin()[2] ) ; extend ( x ) ;
475 sgdSetVec3 ( x, b->getMin()[0],b->getMax()[1],b->getMax()[2] ) ; extend ( x ) ;
476 sgdSetVec3 ( x, b->getMax()[0],b->getMin()[1],b->getMin()[2] ) ; extend ( x ) ;
477 sgdSetVec3 ( x, b->getMax()[0],b->getMin()[1],b->getMax()[2] ) ; extend ( x ) ;
478 sgdSetVec3 ( x, b->getMax()[0],b->getMax()[1],b->getMin()[2] ) ; extend ( x ) ;
479 extend ( b->getMax() ) ;
480 #endif
481 }
482
483
extend(const sgdSphere * s)484 void sgdSphere::extend ( const sgdSphere *s )
485 {
486 if ( s->isEmpty () )
487 return ;
488
489 if ( isEmpty () )
490 {
491 sgdCopyVec3 ( center, s->getCenter() ) ;
492 radius = s->getRadius() ;
493 return ;
494 }
495
496 /*
497 d == The distance between the sphere centers
498 */
499
500 SGDfloat d = sgdDistanceVec3 ( 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 sgdCopyVec3 ( 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 SGDfloat new_radius = (radius + d + s->getRadius() ) / SGD_TWO ;
522
523 SGDfloat 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 sgdBox * b) const532 int sgdSphere::intersects ( const sgdBox *b ) const
533 {
534 sgdVec3 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 sgdCompare3DSqdDist ( closest, center, sgdSquare ( radius ) ) <= 0 ;
549 }
550
551
552 /************************\
553 * sgdFrustum routines *
554 \************************/
555
update()556 void sgdFrustum::update ()
557 {
558 if ( fabs ( ffar - nnear ) < 0.1 )
559 {
560 ulSetError ( UL_WARNING, "sgdFrustum: Can't support depth of view <0.1 units.");
561 return ;
562 }
563
564 if ( hfov != SGD_ZERO && vfov != SGD_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 = SGD_HALF * hfov ;
577 top = SGD_HALF * vfov ;
578 }
579 else
580 {
581 right = nnear * tan ( hfov * SGD_DEGREES_TO_RADIANS / SGD_TWO ) ;
582 top = nnear * tan ( vfov * SGD_DEGREES_TO_RADIANS / SGD_TWO ) ;
583 }
584
585 left = -right ;
586 bot = -top ;
587 }
588
589
590 /* Compute the projection matrix */
591
592 SGDfloat width = right - left ;
593 SGDfloat height = top - bot ;
594 SGDfloat depth = ffar - nnear ;
595
596 if ( ortho )
597 {
598 /* orthographic */
599
600 mat[0][0] = SGD_TWO / width ;
601 mat[0][1] = SGD_ZERO ;
602 mat[0][2] = SGD_ZERO ;
603 mat[0][3] = SGD_ZERO ;
604
605 mat[1][0] = SGD_ZERO ;
606 mat[1][1] = SGD_TWO / height ;
607 mat[1][2] = SGD_ZERO ;
608 mat[1][3] = SGD_ZERO ;
609
610 mat[2][0] = SGD_ZERO ;
611 mat[2][1] = SGD_ZERO ;
612 mat[2][2] = -SGD_TWO / depth ;
613 mat[2][3] = SGD_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] = SGD_ONE ;
619 }
620 else
621 {
622 /* perspective */
623
624 mat[0][0] = SGD_TWO * nnear / width ;
625 mat[0][1] = SGD_ZERO ;
626 mat[0][2] = SGD_ZERO ;
627 mat[0][3] = SGD_ZERO ;
628
629 mat[1][0] = SGD_ZERO ;
630 mat[1][1] = SGD_TWO * nnear / height ;
631 mat[1][2] = SGD_ZERO ;
632 mat[1][3] = SGD_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] = -SGD_ONE ;
638
639 mat[3][0] = SGD_ZERO ;
640 mat[3][1] = SGD_ZERO ;
641 mat[3][2] = -SGD_TWO * nnear * ffar / depth ;
642 mat[3][3] = SGD_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 sgdSetVec4( plane[ SG_LEFT_PLANE ], SGD_ONE, SGD_ZERO, SGD_ZERO, SGD_ONE );
671 sgdSetVec4( plane[ SG_RIGHT_PLANE ], -SGD_ONE, SGD_ZERO, SGD_ZERO, SGD_ONE );
672 sgdSetVec4( plane[ SG_BOT_PLANE ], SGD_ZERO, SGD_ONE, SGD_ZERO, SGD_ONE );
673 sgdSetVec4( plane[ SG_TOP_PLANE ], SGD_ZERO, -SGD_ONE, SGD_ZERO, SGD_ONE );
674 sgdSetVec4( plane[ SG_NEAR_PLANE ], SGD_ZERO, SGD_ZERO, SGD_ONE, SGD_ONE );
675 sgdSetVec4( plane[ SG_FAR_PLANE ], SGD_ZERO, SGD_ZERO, -SGD_ONE, SGD_ONE );
676
677 for ( int i = 0 ; i < 6 ; i++ )
678 {
679 sgdVec4 tmp ;
680
681 for ( int j = 0 ; j < 4 ; j++ )
682 tmp[j] = sgdScalarProductVec4 ( plane[i], mat[j] ) ;
683
684 sgdScaleVec4 ( plane[i], tmp, SGD_ONE / sgdLengthVec3 ( 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 sgdVec3 pt) const701 int sgdFrustum::getOutcode ( const sgdVec3 pt ) const
702 {
703 /* Transform the point by the Frustum's transform. */
704
705 sgdVec4 tmp ;
706
707 tmp [ 0 ] = pt [ 0 ] ;
708 tmp [ 1 ] = pt [ 1 ] ;
709 tmp [ 2 ] = pt [ 2 ] ;
710 tmp [ 3 ] = SGD_ONE ;
711
712 sgdXformPnt4 ( 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 sgdVec3 pt) const727 int sgdFrustum::contains ( const sgdVec3 pt ) const
728 {
729 return getOutcode ( pt ) == OC_ALL_ON_SCREEN ;
730 }
731
732
contains(const sgdSphere * s) const733 int sgdFrustum::contains ( const sgdSphere *s ) const
734 {
735
736 const SGDfloat *center = s->getCenter() ;
737 const SGDfloat 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 sgdFrustumContainsPt - 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 SGDfloat 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 = sgdScalarProductVec3 ( left_plane, center ) + left_plane[3] ;
798 sp2 = sgdScalarProductVec3 ( right_plane, center ) + right_plane[3] ;
799 ...
800 sp6 = sgdScalarProductVec3 ( 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 sgdBox * b) const820 int sgdFrustum::contains ( const sgdBox *b ) const
821 {
822 sgdVec3 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
848
sgdDistSquaredToLineVec3(const sgdLine3 line,const sgdVec3 pnt)849 SGDfloat sgdDistSquaredToLineVec3 ( const sgdLine3 line, const sgdVec3 pnt )
850 {
851 sgdVec3 r ; sgdSubVec3 ( r, pnt, line.point_on_line ) ;
852
853 return sgdScalarProductVec3 ( r, r ) -
854 sgdScalarProductVec3 ( r, line.direction_vector ) ;
855 }
856
857
858
sgdDistSquaredToLineSegmentVec3(const sgdLineSegment3 line,const sgdVec3 pnt)859 SGDfloat sgdDistSquaredToLineSegmentVec3 ( const sgdLineSegment3 line,
860 const sgdVec3 pnt )
861 {
862 sgdVec3 v ; sgdSubVec3 ( v, line.b, line.a ) ;
863 sgdVec3 r1 ; sgdSubVec3 ( r1, pnt, line.a ) ;
864
865 SGDfloat r1_dot_v = sgdScalarProductVec3 ( r1, v ) ;
866
867 if ( r1_dot_v <= 0 ) /* Off the "A" end */
868 return sgdScalarProductVec3 ( r1, r1 ) ;
869
870 sgdVec3 r2 ; sgdSubVec3 ( r2, pnt, line.b ) ;
871
872 SGDfloat r2_dot_v = sgdScalarProductVec3 ( r2, v ) ;
873
874 if ( r2_dot_v >= 0 ) /* Off the "B" end */
875 return sgdScalarProductVec3 ( r2, r2 ) ;
876
877 /* Closest point on line is on the line segment */
878
879 return sgdScalarProductVec3 ( r1, r1 ) - r1_dot_v * r1_dot_v / sgdScalarProductVec3 ( v, v ) ;
880 }
881
882
883
sgdMakeCoordMat4(sgdMat4 m,const SGDfloat x,const SGDfloat y,const SGDfloat z,const SGDfloat h,const SGDfloat p,const SGDfloat r)884 void sgdMakeCoordMat4 ( sgdMat4 m, const SGDfloat x, const SGDfloat y, const SGDfloat z, const SGDfloat h, const SGDfloat p, const SGDfloat r )
885 {
886 SGDfloat ch, sh, cp, sp, cr, sr, srsp, crsp, srcp ;
887
888 if ( h == SGD_ZERO )
889 {
890 ch = SGD_ONE ;
891 sh = SGD_ZERO ;
892 }
893 else
894 {
895 sh = sgdSin( h ) ;
896 ch = sgdCos( h ) ;
897 }
898
899 if ( p == SGD_ZERO )
900 {
901 cp = SGD_ONE ;
902 sp = SGD_ZERO ;
903 }
904 else
905 {
906 sp = sgdSin( p ) ;
907 cp = sgdCos( p ) ;
908 }
909
910 if ( r == SGD_ZERO )
911 {
912 cr = SGD_ONE ;
913 sr = SGD_ZERO ;
914 srsp = SGD_ZERO ;
915 srcp = SGD_ZERO ;
916 crsp = sp ;
917 }
918 else
919 {
920 sr = sgdSin( r ) ;
921 cr = sgdCos( r ) ;
922 srsp = sr * sp ;
923 crsp = cr * sp ;
924 srcp = sr * cp ;
925 }
926
927 m[0][0] = ch * cr - sh * srsp ;
928 m[1][0] = -sh * cp ;
929 m[2][0] = sr * ch + sh * crsp ;
930 m[3][0] = x ;
931
932 m[0][1] = cr * sh + srsp * ch ;
933 m[1][1] = ch * cp ;
934 m[2][1] = sr * sh - crsp * ch ;
935 m[3][1] = y ;
936
937 m[0][2] = -srcp ;
938 m[1][2] = sp ;
939 m[2][2] = cr * cp ;
940 m[3][2] = z ;
941
942 m[0][3] = SGD_ZERO ;
943 m[1][3] = SGD_ZERO ;
944 m[2][3] = SGD_ZERO ;
945 m[3][3] = SGD_ONE ;
946 }
947
948
sgdMakeTransMat4(sgdMat4 m,const sgdVec3 xyz)949 void sgdMakeTransMat4 ( sgdMat4 m, const sgdVec3 xyz )
950 {
951 m[0][1] = m[0][2] = m[0][3] =
952 m[1][0] = m[1][2] = m[1][3] =
953 m[2][0] = m[2][1] = m[2][3] = SGD_ZERO ;
954 m[0][0] = m[1][1] = m[2][2] = m[3][3] = SGD_ONE ;
955 sgdCopyVec3 ( m[3], xyz ) ;
956 }
957
958
sgdMakeTransMat4(sgdMat4 m,const SGDfloat x,const SGDfloat y,const SGDfloat z)959 void sgdMakeTransMat4 ( sgdMat4 m, const SGDfloat x, const SGDfloat y, const SGDfloat z )
960 {
961 m[0][1] = m[0][2] = m[0][3] =
962 m[1][0] = m[1][2] = m[1][3] =
963 m[2][0] = m[2][1] = m[2][3] = SGD_ZERO ;
964 m[0][0] = m[1][1] = m[2][2] = m[3][3] = SGD_ONE ;
965 sgdSetVec3 ( m[3], x, y, z ) ;
966 }
967
968
sgdSetCoord(sgdCoord * dst,const sgdMat4 src)969 void sgdSetCoord ( sgdCoord *dst, const sgdMat4 src )
970 {
971 sgdCopyVec3 ( dst->xyz, src[3] ) ;
972
973 sgdMat4 mat ;
974
975 SGDfloat s = sgdLengthVec3 ( src[0] ) ;
976
977 if ( s <= 0.00001 )
978 {
979 ulSetError ( UL_WARNING, "sgdMat4ToCoord: ERROR - Bad Matrix." ) ;
980 sgdSetVec3 ( dst -> hpr, SGD_ZERO, SGD_ZERO, SGD_ZERO ) ;
981 return ;
982 }
983
984 sgdScaleMat4 ( mat, src, SGD_ONE / s ) ;
985
986 dst->hpr[1] = sgdASin ( _sgdClampToUnity ( mat[1][2] ) ) ;
987
988 SGDfloat cp = sgdCos ( dst->hpr[1] ) ;
989
990 /* If pointing nearly vertically up - then heading is ill-defined */
991
992 if ( cp > -0.00001 && cp < 0.00001 )
993 {
994 SGDfloat cr = _sgdClampToUnity ( mat[0][1] ) ;
995 SGDfloat sr = _sgdClampToUnity (-mat[2][1] ) ;
996
997 dst->hpr[0] = SGD_ZERO ;
998 dst->hpr[2] = sgdATan2 ( sr, cr ) ;
999 }
1000 else
1001 {
1002 cp = SGD_ONE / cp ;
1003 SGDfloat sr = _sgdClampToUnity ( -mat[0][2] * cp ) ;
1004 SGDfloat cr = _sgdClampToUnity ( mat[2][2] * cp ) ;
1005 SGDfloat sh = _sgdClampToUnity ( -mat[1][0] * cp ) ;
1006 SGDfloat ch = _sgdClampToUnity ( mat[1][1] * cp ) ;
1007
1008 if ( (sh == SGD_ZERO && ch == SGD_ZERO) || (sr == SGD_ZERO && cr == SGD_ZERO) )
1009 {
1010 cr = _sgdClampToUnity ( mat[0][1] ) ;
1011 sr = _sgdClampToUnity (-mat[2][1] ) ;
1012
1013 dst->hpr[0] = SGD_ZERO ;
1014 }
1015 else
1016 dst->hpr[0] = sgdATan2 ( sh, ch ) ;
1017
1018 dst->hpr[2] = sgdATan2 ( sr, cr ) ;
1019 }
1020 }
1021
1022
sgdMakeNormal(sgdVec2 dst,const sgdVec2 a,const sgdVec2 b)1023 void sgdMakeNormal(sgdVec2 dst, const sgdVec2 a, const sgdVec2 b )
1024 {
1025 sgdSubVec2 ( dst, b, a ) ;
1026 SGDfloat tmp = dst [ 0 ] ; dst [ 0 ] = -dst [ 1 ] ; dst [ 1 ] = tmp ;
1027 sgdNormaliseVec2 ( dst ) ;
1028 }
1029
1030
sgdMakeNormal(sgdVec3 dst,const sgdVec3 a,const sgdVec3 b,const sgdVec3 c)1031 void sgdMakeNormal(sgdVec3 dst, const sgdVec3 a, const sgdVec3 b, const sgdVec3 c )
1032 {
1033 sgdVec3 ab ; sgdSubVec3 ( ab, b, a ) ;
1034 sgdVec3 ac ; sgdSubVec3 ( ac, c, a ) ;
1035 sgdVectorProductVec3 ( dst, ab,ac ) ; sgdNormaliseVec3 ( dst ) ;
1036 }
1037
1038
sgdPreMultMat4(sgdMat4 dst,const sgdMat4 src)1039 void sgdPreMultMat4( sgdMat4 dst, const sgdMat4 src )
1040 {
1041 sgdMat4 mat ;
1042 sgdMultMat4 ( mat, dst, src ) ;
1043 sgdCopyMat4 ( dst, mat ) ;
1044 }
1045
sgdPostMultMat4(sgdMat4 dst,const sgdMat4 src)1046 void sgdPostMultMat4( sgdMat4 dst, const sgdMat4 src )
1047 {
1048 sgdMat4 mat ;
1049 sgdMultMat4 ( mat, src, dst ) ;
1050 sgdCopyMat4 ( dst, mat ) ;
1051 }
1052
sgdMultMat4(sgdMat4 dst,const sgdMat4 m1,const sgdMat4 m2)1053 void sgdMultMat4( sgdMat4 dst, const sgdMat4 m1, const sgdMat4 m2 )
1054 {
1055 for ( int j = 0 ; j < 4 ; j++ )
1056 {
1057 dst[0][j] = m2[0][0] * m1[0][j] +
1058 m2[0][1] * m1[1][j] +
1059 m2[0][2] * m1[2][j] +
1060 m2[0][3] * m1[3][j] ;
1061
1062 dst[1][j] = m2[1][0] * m1[0][j] +
1063 m2[1][1] * m1[1][j] +
1064 m2[1][2] * m1[2][j] +
1065 m2[1][3] * m1[3][j] ;
1066
1067 dst[2][j] = m2[2][0] * m1[0][j] +
1068 m2[2][1] * m1[1][j] +
1069 m2[2][2] * m1[2][j] +
1070 m2[2][3] * m1[3][j] ;
1071
1072 dst[3][j] = m2[3][0] * m1[0][j] +
1073 m2[3][1] * m1[1][j] +
1074 m2[3][2] * m1[2][j] +
1075 m2[3][3] * m1[3][j] ;
1076 }
1077 }
1078
1079
sgdTransposeNegateMat4(sgdMat4 dst,const sgdMat4 src)1080 void sgdTransposeNegateMat4 ( sgdMat4 dst, const sgdMat4 src )
1081 {
1082 /* Poor man's invert - can be used when matrix is a simple rotate-translate */
1083
1084 dst[0][0] = src[0][0] ;
1085 dst[1][0] = src[0][1] ;
1086 dst[2][0] = src[0][2] ;
1087 dst[3][0] = - sgdScalarProductVec3 ( src[3], src[0] ) ;
1088
1089 dst[0][1] = src[1][0] ;
1090 dst[1][1] = src[1][1] ;
1091 dst[2][1] = src[1][2] ;
1092 dst[3][1] = - sgdScalarProductVec3 ( src[3], src[1] ) ;
1093
1094 dst[0][2] = src[2][0] ;
1095 dst[1][2] = src[2][1] ;
1096 dst[2][2] = src[2][2] ;
1097 dst[3][2] = - sgdScalarProductVec3 ( src[3], src[2] ) ;
1098
1099 dst[0][3] = SGD_ZERO ;
1100 dst[1][3] = SGD_ZERO ;
1101 dst[2][3] = SGD_ZERO ;
1102 dst[3][3] = SGD_ONE ;
1103 }
1104
1105
sgdTransposeNegateMat4(sgdMat4 dst)1106 void sgdTransposeNegateMat4 ( sgdMat4 dst )
1107 {
1108 sgdMat4 src ;
1109 sgdCopyMat4 ( src, dst ) ;
1110 sgdTransposeNegateMat4 ( dst, src ) ;
1111 }
1112
1113
1114
sgdInvertMat4(sgdMat4 dst,const sgdMat4 src)1115 void sgdInvertMat4 ( sgdMat4 dst, const sgdMat4 src )
1116 {
1117 sgdMat4 tmp ;
1118
1119 sgdCopyMat4 ( tmp, src ) ;
1120 sgdMakeIdentMat4 ( dst ) ;
1121
1122 for ( int i = 0 ; i != 4 ; i++ )
1123 {
1124 SGDfloat val = tmp[i][i] ;
1125 int ind = i ;
1126 int j ;
1127
1128 for ( j = i + 1 ; j != 4 ; j++ )
1129 {
1130 if ( fabs ( tmp[i][j] ) > fabs(val) )
1131 {
1132 ind = j;
1133 val = tmp[i][j] ;
1134 }
1135 }
1136
1137 if ( ind != i )
1138 { /* swap columns */
1139 for ( j = 0 ; j != 4 ; j++ )
1140 {
1141 SGDfloat t ;
1142 t = dst[j][i]; dst[j][i] = dst[j][ind]; dst[j][ind] = t ;
1143 t = tmp[j][i]; tmp[j][i] = tmp[j][ind]; tmp[j][ind] = t ;
1144 }
1145 }
1146
1147 // if ( val == SG_ZERO)
1148 if ( fabs(val) <= DBL_EPSILON )
1149 {
1150 ulSetError ( UL_WARNING, "sg: ERROR - Singular matrix, no inverse!" ) ;
1151 sgdMakeIdentMat4 ( dst ) ; /* Do *something* */
1152 return;
1153 }
1154
1155 SGDfloat ival = SGD_ONE / val ;
1156
1157 for ( j = 0 ; j != 4 ; j++ )
1158 {
1159 tmp[j][i] *= ival ;
1160 dst[j][i] *= ival ;
1161 }
1162
1163 for (j = 0; j != 4; j++)
1164 {
1165 if ( j == i )
1166 continue ;
1167
1168 val = tmp[i][j] ;
1169
1170 for ( int k = 0 ; k != 4 ; k++ )
1171 {
1172 tmp[k][j] -= tmp[k][i] * val ;
1173 dst[k][j] -= dst[k][i] * val ;
1174 }
1175 }
1176 }
1177 }
1178
1179
1180
sgdXformVec3(sgdVec3 dst,const sgdVec3 src,const sgdMat4 mat)1181 void sgdXformVec3 ( sgdVec3 dst, const sgdVec3 src, const sgdMat4 mat )
1182 {
1183 SGDfloat t0 = src[ 0 ] ;
1184 SGDfloat t1 = src[ 1 ] ;
1185 SGDfloat t2 = src[ 2 ] ;
1186
1187 dst[0] = ( t0 * mat[ 0 ][ 0 ] +
1188 t1 * mat[ 1 ][ 0 ] +
1189 t2 * mat[ 2 ][ 0 ] ) ;
1190
1191 dst[1] = ( t0 * mat[ 0 ][ 1 ] +
1192 t1 * mat[ 1 ][ 1 ] +
1193 t2 * mat[ 2 ][ 1 ] ) ;
1194
1195 dst[2] = ( t0 * mat[ 0 ][ 2 ] +
1196 t1 * mat[ 1 ][ 2 ] +
1197 t2 * mat[ 2 ][ 2 ] ) ;
1198 }
1199
1200
sgdXformPnt3(sgdVec3 dst,const sgdVec3 src,const sgdMat4 mat)1201 void sgdXformPnt3 ( sgdVec3 dst, const sgdVec3 src, const sgdMat4 mat )
1202 {
1203 SGDfloat t0 = src[ 0 ] ;
1204 SGDfloat t1 = src[ 1 ] ;
1205 SGDfloat t2 = src[ 2 ] ;
1206
1207 dst[0] = ( t0 * mat[ 0 ][ 0 ] +
1208 t1 * mat[ 1 ][ 0 ] +
1209 t2 * mat[ 2 ][ 0 ] +
1210 mat[ 3 ][ 0 ] ) ;
1211
1212 dst[1] = ( t0 * mat[ 0 ][ 1 ] +
1213 t1 * mat[ 1 ][ 1 ] +
1214 t2 * mat[ 2 ][ 1 ] +
1215 mat[ 3 ][ 1 ] ) ;
1216
1217 dst[2] = ( t0 * mat[ 0 ][ 2 ] +
1218 t1 * mat[ 1 ][ 2 ] +
1219 t2 * mat[ 2 ][ 2 ] +
1220 mat[ 3 ][ 2 ] ) ;
1221 }
1222
1223
sgdXformPnt4(sgdVec4 dst,const sgdVec4 src,const sgdMat4 mat)1224 void sgdXformPnt4 ( sgdVec4 dst, const sgdVec4 src, const sgdMat4 mat )
1225 {
1226 SGDfloat t0 = src[ 0 ] ;
1227 SGDfloat t1 = src[ 1 ] ;
1228 SGDfloat t2 = src[ 2 ] ;
1229 SGDfloat t3 = src[ 3 ] ;
1230
1231 dst[0] = ( t0 * mat[ 0 ][ 0 ] +
1232 t1 * mat[ 1 ][ 0 ] +
1233 t2 * mat[ 2 ][ 0 ] +
1234 t3 * mat[ 3 ][ 0 ] ) ;
1235
1236 dst[1] = ( t0 * mat[ 0 ][ 1 ] +
1237 t1 * mat[ 1 ][ 1 ] +
1238 t2 * mat[ 2 ][ 1 ] +
1239 t3 * mat[ 3 ][ 1 ] ) ;
1240
1241 dst[2] = ( t0 * mat[ 0 ][ 2 ] +
1242 t1 * mat[ 1 ][ 2 ] +
1243 t2 * mat[ 2 ][ 2 ] +
1244 t3 * mat[ 3 ][ 2 ] ) ;
1245
1246 dst[3] = ( t0 * mat[ 0 ][ 3 ] +
1247 t1 * mat[ 1 ][ 3 ] +
1248 t2 * mat[ 2 ][ 3 ] +
1249 t3 * mat[ 3 ][ 3 ] ) ;
1250 }
1251
1252
sgdFullXformPnt3(sgdVec3 dst,const sgdVec3 src,const sgdMat4 mat)1253 void sgdFullXformPnt3 ( sgdVec3 dst, const sgdVec3 src, const sgdMat4 mat )
1254 {
1255 sgdVec4 tmp ;
1256
1257 tmp [ 0 ] = src [ 0 ] ;
1258 tmp [ 1 ] = src [ 1 ] ;
1259 tmp [ 2 ] = src [ 2 ] ;
1260 tmp [ 3 ] = SGD_ONE ;
1261
1262 sgdXformPnt4 ( tmp, tmp, mat ) ;
1263 sgdScaleVec3 ( dst, tmp, SGD_ONE / tmp [ 3 ] ) ;
1264 }
1265
sgdHPRfromVec3(sgdVec3 hpr,sgdVec3 src)1266 void sgdHPRfromVec3 ( sgdVec3 hpr, sgdVec3 src )
1267 {
1268 sgdVec3 tmp ;
1269 sgdCopyVec3 ( tmp, src ) ;
1270 sgdNormaliseVec3 ( tmp ) ;
1271 hpr[0] = - sgdATan2 ( tmp [ 0 ], tmp [ 1 ] ) ;
1272 hpr[1] = - sgdATan2 ( tmp [ 2 ], sgdSqrt ( sgdSquare ( tmp [ 0 ] ) +
1273 sgdSquare ( tmp [ 1 ] ) ) ) ;
1274 hpr[2] = SGD_ZERO ;
1275 }
1276
1277
1278
1279 /*
1280 Quaternion routines are Copyright (C) 1999
1281 Kevin B. Thompson <kevinbthompson@yahoo.com>
1282 Modified by Sylvan W. Clebsch <sylvan@stanford.edu>
1283 Largely rewritten by "Negative0" <negative0@earthlink.net>
1284 */
1285
1286
sgdQuatToAngleAxis(SGDfloat * angle,SGDfloat * x,SGDfloat * y,SGDfloat * z,const sgdQuat src)1287 void sgdQuatToAngleAxis ( SGDfloat *angle,
1288 SGDfloat *x, SGDfloat *y, SGDfloat *z,
1289 const sgdQuat src )
1290 {
1291 sgdVec3 axis ;
1292
1293 sgdQuatToAngleAxis ( angle, axis, src ) ;
1294
1295 *x = axis [ 0 ] ;
1296 *y = axis [ 1 ] ;
1297 *z = axis [ 2 ] ;
1298 }
1299
1300
sgdQuatToAngleAxis(SGDfloat * angle,sgdVec3 axis,const sgdQuat src)1301 void sgdQuatToAngleAxis ( SGDfloat *angle, sgdVec3 axis, const sgdQuat src )
1302 {
1303 SGDfloat a = (SGDfloat) acos ( src[SGD_W] ) ;
1304 SGDfloat s = (SGDfloat) sin ( a ) ;
1305
1306 *angle = a * SGD_RADIANS_TO_DEGREES * SGD_TWO ;
1307
1308 if ( s == SGD_ZERO )
1309 sgdSetVec3 ( axis, SGD_ZERO, SGD_ZERO, SGD_ONE );
1310 else
1311 {
1312 sgdSetVec3 ( axis, src[SGD_X], src[SGD_Y], src[SGD_Z] ) ;
1313 sgdScaleVec3 ( axis, SGD_ONE / s ) ;
1314 }
1315 }
1316
1317
sgdAngleAxisToQuat(sgdQuat dst,const SGDfloat angle,const SGDfloat x,const SGDfloat y,const SGDfloat z)1318 void sgdAngleAxisToQuat ( sgdQuat dst,
1319 const SGDfloat angle,
1320 const SGDfloat x, const SGDfloat y, const SGDfloat z )
1321 {
1322 sgdVec3 axis ;
1323 sgdSetVec3 ( axis, x, y, z ) ;
1324 sgdAngleAxisToQuat ( dst, angle, axis ) ;
1325 }
1326
1327
sgdAngleAxisToQuat(sgdQuat dst,const SGDfloat angle,const sgdVec3 axis)1328 void sgdAngleAxisToQuat ( sgdQuat dst, const SGDfloat angle, const sgdVec3 axis )
1329 {
1330 SGDfloat temp_angle = angle * SGD_DEGREES_TO_RADIANS / SGD_TWO ;
1331
1332 sgdVec3 ax ;
1333 sgdNormaliseVec3 ( ax, axis ) ;
1334
1335 SGDfloat s = - (SGDfloat) sin ( temp_angle ) ;
1336
1337 dst[SGD_W] = (SGDfloat) cos ( temp_angle ) ;
1338 sgdScaleVec3 ( dst, ax, s ) ;
1339 }
1340
1341
1342 //from gamasutra.com
1343 //by nb
1344
sgdMatrixToQuat(sgdQuat quat,const sgdMat4 m)1345 void sgdMatrixToQuat( sgdQuat quat, const sgdMat4 m )
1346 {
1347 SGDfloat tr, s, q[4] ;
1348 int i, j, k ;
1349
1350 int nxt[3] = {1, 2, 0};
1351
1352 tr = m[0][0] + m[1][1] + m[2][2];
1353
1354 // check the diagonal
1355 if (tr > SGD_ZERO )
1356 {
1357 s = (SGDfloat) sqrt (tr + SGD_ONE);
1358 quat[SGD_W] = s / SGD_TWO;
1359 s = SGD_HALF / s;
1360 quat[SGD_X] = (m[1][2] - m[2][1]) * s;
1361 quat[SGD_Y] = (m[2][0] - m[0][2]) * s;
1362 quat[SGD_Z] = (m[0][1] - m[1][0]) * s;
1363 }
1364 else
1365 {
1366 // diagonal is negative
1367 i = 0;
1368 if (m[1][1] > m[0][0]) i = 1;
1369 if (m[2][2] > m[i][i]) i = 2;
1370 j = nxt[i];
1371 k = nxt[j];
1372 s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + SGD_ONE);
1373 q[i] = s * SGD_HALF;
1374
1375 if (s != SGD_ZERO) s = SGD_HALF / s;
1376
1377 q[3] = (m[j][k] - m[k][j]) * s;
1378 q[j] = (m[i][j] + m[j][i]) * s;
1379 q[k] = (m[i][k] + m[k][i]) * s;
1380
1381 quat[SGD_X] = q[0];
1382 quat[SGD_Y] = q[1];
1383 quat[SGD_Z] = q[2];
1384 quat[SGD_W] = q[3];
1385 }
1386
1387 // seems to yield the inverse rotation, so:
1388 quat[SG_W] = - quat[SG_W];
1389 }
1390
1391
sgdMultQuat(sgdQuat dst,const sgdQuat a,const sgdQuat b)1392 void sgdMultQuat ( sgdQuat dst, const sgdQuat a, const sgdQuat b )
1393 {
1394 /* [ ww' - v.v', vxv' + wv' + v'w ] */
1395
1396 SGDfloat t[8];
1397
1398 t[0] = (a[SGD_W] + a[SGD_X]) * (b[SGD_W] + b[SGD_X]);
1399 t[1] = (a[SGD_Z] - a[SGD_Y]) * (b[SGD_Y] - b[SGD_Z]);
1400 t[2] = (a[SGD_X] - a[SGD_W]) * (b[SGD_Y] + b[SGD_Z]);
1401 t[3] = (a[SGD_Y] + a[SGD_Z]) * (b[SGD_X] - b[SGD_W]);
1402 t[4] = (a[SGD_X] + a[SGD_Z]) * (b[SGD_X] + b[SGD_Y]);
1403 t[5] = (a[SGD_X] - a[SGD_Z]) * (b[SGD_X] - b[SGD_Y]);
1404 t[6] = (a[SGD_W] + a[SGD_Y]) * (b[SGD_W] - b[SGD_Z]);
1405 t[7] = (a[SGD_W] - a[SGD_Y]) * (b[SGD_W] + b[SGD_Z]);
1406
1407 dst[SGD_W] = t[1] + ((-t[4] - t[5] + t[6] + t[7]) * SGD_HALF);
1408 dst[SGD_X] = t[0] - (( t[4] + t[5] + t[6] + t[7]) * SGD_HALF);
1409 dst[SGD_Y] = -t[2] + (( t[4] - t[5] + t[6] - t[7]) * SGD_HALF);
1410 dst[SGD_Z] = -t[3] + (( t[4] - t[5] - t[6] + t[7]) * SGD_HALF);
1411 }
1412
1413 //from gamasutra.com
1414 //by nb@netcom.ca
1415
sgdMultQuat2(sgdQuat dst,const sgdQuat a,const sgdQuat b)1416 void sgdMultQuat2 ( sgdQuat dst, const sgdQuat a, const sgdQuat b )
1417 {
1418 SGDfloat A, B, C, D, E, F, G, H;
1419
1420 A = (a[SGD_W] + a[SGD_X]) * (b[SGD_W] + b[SGD_X]) ;
1421 B = (a[SGD_Z] - a[SGD_Y]) * (b[SGD_Y] - b[SGD_Z]) ;
1422 C = (a[SGD_X] - a[SGD_W]) * (b[SGD_Y] + b[SGD_Z]) ;
1423 D = (a[SGD_Y] + a[SGD_Z]) * (b[SGD_X] - b[SGD_W]) ;
1424 E = (a[SGD_X] + a[SGD_Z]) * (b[SGD_X] + b[SGD_Y]) ;
1425 F = (a[SGD_X] - a[SGD_Z]) * (b[SGD_X] - b[SGD_Y]) ;
1426 G = (a[SGD_W] + a[SGD_Y]) * (b[SGD_W] - b[SGD_Z]) ;
1427 H = (a[SGD_W] - a[SGD_Y]) * (b[SGD_W] + b[SGD_Z]) ;
1428
1429
1430 dst[SGD_W] = B + (-E - F + G + H) / SGD_TWO ;
1431 dst[SGD_X] = A - ( E + F + G + H) / SGD_TWO ;
1432 dst[SGD_Y] = -C + ( E - F + G - H) / SGD_TWO ;
1433 dst[SGD_Z] = -D + ( E - F - G + H) / SGD_TWO ;
1434 }
1435
1436 //from gamasutra.com
1437 //by nb@netcom.ca
1438
sgdEulerToQuat(sgdQuat quat,const sgdVec3 hpr)1439 void sgdEulerToQuat(sgdQuat quat, const sgdVec3 hpr )
1440 {
1441 SGDfloat cr, cp, cy, sr, sp, sy, cpcy, spsy;
1442
1443 // calculate trig identities
1444 cr = (SGDfloat) cos(hpr[2]*SGD_DEGREES_TO_RADIANS/SGD_TWO);
1445 cp = (SGDfloat) cos(hpr[1]*SGD_DEGREES_TO_RADIANS/SGD_TWO);
1446 cy = (SGDfloat) cos(hpr[0]*SGD_DEGREES_TO_RADIANS/SGD_TWO);
1447
1448 sr = (SGDfloat) sin(hpr[2]*SGD_DEGREES_TO_RADIANS/SGD_TWO);
1449 sp = (SGDfloat) sin(hpr[1]*SGD_DEGREES_TO_RADIANS/SGD_TWO);
1450 sy = (SGDfloat) sin(hpr[0]*SGD_DEGREES_TO_RADIANS/SGD_TWO);
1451
1452 cpcy = cp * cy;
1453 spsy = sp * sy;
1454
1455 quat[SGD_W] = cr * cpcy + sr * spsy;
1456 quat[SGD_X] = sr * cpcy - cr * spsy;
1457 quat[SGD_Y] = cr * sp * cy + sr * cp * sy;
1458 quat[SGD_Z] = cr * cp * sy - sr * sp * cy;
1459 }
1460
1461 //from darwin3d.com
1462 // jeffl@darwin3d.com
1463
sgdQuatToEuler(sgdVec3 hpr,const sgdQuat quat)1464 void sgdQuatToEuler( sgdVec3 hpr, const sgdQuat quat )
1465 {
1466 SGDfloat matrix[3][3];
1467 SGDfloat cx,sx;
1468 SGDfloat cy,sy;
1469 SGDfloat cz,sz;
1470
1471 // CONVERT QUATERNION TO MATRIX - I DON'T REALLY NEED ALL OF IT
1472
1473 matrix[0][0] = SGD_ONE - (SGD_TWO * quat[SGD_Y] * quat[SGD_Y])
1474 - (SGD_TWO * quat[SGD_Z] * quat[SGD_Z]);
1475 //matrix[0][1] = (SGD_TWO * quat->x * quat->y) - (SGD_TWO * quat->w * quat->z);
1476 //matrix[0][2] = (SGD_TWO * quat->x * quat->z) + (SGD_TWO * quat->w * quat->y);
1477
1478 matrix[1][0] = (SGD_TWO * quat[SGD_X] * quat[SGD_Y]) +
1479 (SGD_TWO * quat[SGD_W] * quat[SGD_Z]);
1480 //matrix[1][1] = SGD_ONE - (SGD_TWO * quat->x * quat->x)
1481 // - (SGD_TWO * quat->z * quat->z);
1482 //matrix[1][2] = (SGD_TWO * quat->y * quat->z) - (SGD_TWO * quat->w * quat->x);
1483
1484 matrix[2][0] = (SGD_TWO * quat[SGD_X] * quat[SGD_Z]) -
1485 (SGD_TWO * quat[SGD_W] * quat[SGD_Y]);
1486 matrix[2][1] = (SGD_TWO * quat[SGD_Y] * quat[SGD_Z]) +
1487 (SGD_TWO * quat[SGD_W] * quat[SGD_X]);
1488 matrix[2][2] = SGD_ONE - (SGD_TWO * quat[SGD_X] * quat[SGD_X])
1489 - (SGD_TWO * quat[SGD_Y] * quat[SGD_Y]);
1490
1491 sy = -matrix[2][0];
1492 cy = sgdSqrt(SGD_ONE - (sy * sy));
1493
1494 hpr[1] = sgdATan2( sy, cy );
1495
1496 // AVOID DIVIDE BY ZERO ERROR ONLY WHERE Y= +-90 or +-270
1497 // NOT CHECKING cy BECAUSE OF PRECISION ERRORS
1498 if (sy != SGD_ONE && sy != -SGD_ONE)
1499 {
1500 cx = matrix[2][2] / cy;
1501 sx = matrix[2][1] / cy;
1502 hpr[0] = sgdATan2 ( sx, cx );
1503
1504 cz = matrix[0][0] / cy;
1505 sz = matrix[1][0] / cy;
1506 hpr[2] = sgdATan2 ( sz, cz );
1507 }
1508 else
1509 {
1510 // SINCE Cos(Y) IS 0, I AM SCREWED. ADOPT THE STANDARD Z = 0
1511 // I THINK THERE IS A WAY TO FIX THIS BUT I AM NOT SURE. EULERS SUCK
1512 // NEED SOME MORE OF THE MATRIX TERMS NOW
1513
1514 matrix[1][1] = SGD_ONE - (SGD_TWO * quat[SGD_X] * quat[SGD_X])
1515 - (SGD_TWO * quat[SGD_Z] * quat[SGD_Z]);
1516 matrix[1][2] = (SGD_TWO * quat[SGD_Y] * quat[SGD_Z]) -
1517 (SGD_TWO * quat[SGD_W] * quat[SGD_X]);
1518
1519 cx = matrix[1][1];
1520 sx = -matrix[1][2];
1521 hpr[0] = sgdATan2 ( sx, cx );
1522
1523 cz = SGD_ONE ;
1524 sz = SGD_ZERO ;
1525 hpr[2] = sgdATan2 ( sz, cz );
1526 }
1527 }
1528
1529
sgdQuatToMatrix(sgdMat4 dst,const sgdQuat q)1530 void sgdQuatToMatrix ( sgdMat4 dst, const sgdQuat q )
1531 {
1532 SGDfloat two_xx = q[SGD_X] * (q[SGD_X] + q[SGD_X]) ;
1533 SGDfloat two_xy = q[SGD_X] * (q[SGD_Y] + q[SGD_Y]) ;
1534 SGDfloat two_xz = q[SGD_X] * (q[SGD_Z] + q[SGD_Z]) ;
1535
1536 SGDfloat two_wx = q[SGD_W] * (q[SGD_X] + q[SGD_X]) ;
1537 SGDfloat two_wy = q[SGD_W] * (q[SGD_Y] + q[SGD_Y]) ;
1538 SGDfloat two_wz = q[SGD_W] * (q[SGD_Z] + q[SGD_Z]) ;
1539
1540 SGDfloat two_yy = q[SGD_Y] * (q[SGD_Y] + q[SGD_Y]) ;
1541 SGDfloat two_yz = q[SGD_Y] * (q[SGD_Z] + q[SGD_Z]) ;
1542
1543 SGDfloat two_zz = q[SGD_Z] * (q[SGD_Z] + q[SGD_Z]) ;
1544
1545 sgdSetVec4 ( dst[0], SGD_ONE-(two_yy+two_zz), two_xy-two_wz, two_xz+two_wy, SGD_ZERO ) ;
1546 sgdSetVec4 ( dst[1], two_xy+two_wz, SGD_ONE-(two_xx+two_zz), two_yz-two_wx, SGD_ZERO ) ;
1547 sgdSetVec4 ( dst[2], two_xz-two_wy, two_yz+two_wx, SGD_ONE-(two_xx+two_yy), SGD_ZERO ) ;
1548 sgdSetVec4 ( dst[3], SGD_ZERO, SGD_ZERO, SGD_ZERO, SGD_ONE ) ;
1549 }
1550
1551
1552 //from gamasutra.com
1553 //by nb@netcom.ca
1554
1555 /************************************
1556 DEPRECATED - use sgdQuatToMatrix instead.
1557 *************************************/
1558
sgdMakeRotMat42(sgdMat4 m,sgdQuat quat)1559 void sgdMakeRotMat42( sgdMat4 m, sgdQuat quat ){
1560 SGDfloat wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
1561
1562 // calculate coefficients
1563 x2 = quat[SGD_X] + quat[SGD_X]; y2 = quat[SGD_Y] + quat[SGD_Y];
1564 z2 = quat[SGD_Z] + quat[SGD_Z];
1565 xx = quat[SGD_X] * x2; xy = quat[SGD_X] * y2; xz = quat[SGD_X] * z2;
1566 yy = quat[SGD_Y] * y2; yz = quat[SGD_Y] * z2; zz = quat[SGD_Z] * z2;
1567 wx = quat[SGD_W] * x2; wy = quat[SGD_W] * y2; wz = quat[SGD_W] * z2;
1568
1569 m[0][0] = SGD_ONE- (yy + zz); m[0][1] = xy - wz;
1570 m[0][2] = xz + wy; m[0][3] = SGD_ZERO ;
1571
1572 m[1][0] = xy + wz; m[1][1] = SGD_ONE- (xx + zz);
1573 m[1][2] = yz - wx; m[1][3] = SGD_ZERO ;
1574
1575 m[2][0] = xz - wy; m[2][1] = yz + wx;
1576 m[2][2] = SGD_ONE- (xx + yy); m[2][3] = SGD_ZERO ;
1577
1578 m[3][0] = 0; m[3][1] = 0;
1579 m[3][2] = 0; m[3][3] = 1;
1580 }
1581
1582 //from gamasutra.com
1583 //by nb@netcom.ca
1584
sgdSlerpQuat2(sgdQuat dst,const sgdQuat from,const sgdQuat to,const SGDfloat t)1585 void sgdSlerpQuat2( sgdQuat dst, const sgdQuat from, const sgdQuat to, const SGDfloat t )
1586 {
1587 SGDfloat to1[4];
1588 SGDfloat omega, cosom, sinom, scale0, scale1;
1589
1590 // calc cosine
1591 cosom = from[SGD_X] * to[SGD_X] +
1592 from[SGD_Y] * to[SGD_Y] +
1593 from[SGD_Z] * to[SGD_Z] +
1594 from[SGD_W] * to[SGD_W];
1595
1596 // adjust signs (if necessary)
1597
1598 if ( cosom < SG_ZERO )
1599 {
1600 cosom = -cosom;
1601 to1[0] = - to[SGD_X];
1602 to1[1] = - to[SGD_Y];
1603 to1[2] = - to[SGD_Z];
1604 to1[3] = - to[SGD_W];
1605 }
1606 else
1607 {
1608 to1[0] = to[SGD_X];
1609 to1[1] = to[SGD_Y];
1610 to1[2] = to[SGD_Z];
1611 to1[3] = to[SGD_W];
1612 }
1613
1614 // calculate coefficients
1615 #define DELTA SGD_ZERO
1616 if ( (SGD_ONE- cosom) > DELTA ) {
1617 // standard case (slerp)
1618 omega = acos(cosom);
1619 sinom = sin(omega);
1620 scale0 = sin((SGD_ONE- t) * omega) / sinom;
1621 scale1 = sin(t * omega) / sinom;
1622
1623 }
1624 else
1625 {
1626 // "from" and "to" quaternions are very close
1627 // ... so we can do a linear interpolation
1628 scale0 = SGD_ONE- t;
1629 scale1 = t;
1630 }
1631
1632 // calculate final values
1633 dst[SGD_X] = scale0 * from[SGD_X] + scale1 * to1[0];
1634 dst[SGD_Y] = scale0 * from[SGD_Y] + scale1 * to1[1];
1635 dst[SGD_Z] = scale0 * from[SGD_Z] + scale1 * to1[2];
1636 dst[SGD_W] = scale0 * from[SGD_W] + scale1 * to1[3];
1637 }
1638
sgdSlerpQuat(sgdQuat dst,const sgdQuat from,const sgdQuat to,const SGDfloat t)1639 void sgdSlerpQuat( sgdQuat dst, const sgdQuat from, const sgdQuat to, const SGDfloat t )
1640 {
1641 SGDfloat co, scale0, scale1;
1642 bool flip = false ;
1643
1644 /* SWC - Interpolate between to quaternions */
1645
1646 co = sgdScalarProductVec4 ( from, to ) ;
1647
1648 if ( co < SGD_ZERO )
1649 {
1650 co = -co;
1651 flip = true ;
1652 }
1653
1654 if ( co < SGD_ONE - (SGDfloat) 1e-6 )
1655 {
1656 SGDfloat o = (SGDfloat) acos ( co );
1657 SGDfloat so = SGD_ONE / (SGDfloat) sin ( o );
1658
1659 scale0 = (SGDfloat) sin ( (SGD_ONE - t) * o ) * so;
1660 scale1 = (SGDfloat) sin ( t * o ) * so;
1661 }
1662 else
1663 {
1664 scale0 = SGD_ONE - t;
1665 scale1 = t;
1666 }
1667
1668 if ( flip )
1669 {
1670 scale1 = -scale1 ;
1671 }
1672
1673 dst[SGD_X] = scale0 * from[SGD_X] + scale1 * to[SGD_X] ;
1674 dst[SGD_Y] = scale0 * from[SGD_Y] + scale1 * to[SGD_Y] ;
1675 dst[SGD_Z] = scale0 * from[SGD_Z] + scale1 * to[SGD_Z] ;
1676 dst[SGD_W] = scale0 * from[SGD_W] + scale1 * to[SGD_W] ;
1677 }
1678
1679
1680
1681
1682 /* Function to rotate a vector through a given quaternion using the formula
1683 * R = Q r Q-1 -- this gives the components of a ROTATED vector in a STATIONARY
1684 * coordinate system. We assume that Q is a unit quaternion.
1685 */
sgdRotateVecQuat(sgdVec3 vec,sgdQuat q)1686 void sgdRotateVecQuat ( sgdVec3 vec, sgdQuat q )
1687 {
1688 sgdVec3 rot ;
1689 sgdFloat qwqw = q[SG_W] * q[SG_W] ;
1690 sgdFloat qwqx = q[SG_W] * q[SG_X] ;
1691 sgdFloat qwqy = q[SG_W] * q[SG_Y] ;
1692 sgdFloat qwqz = q[SG_W] * q[SG_Z] ;
1693 sgdFloat qxqx = q[SG_X] * q[SG_X] ;
1694 sgdFloat qxqy = q[SG_X] * q[SG_Y] ;
1695 sgdFloat qxqz = q[SG_X] * q[SG_Z] ;
1696 sgdFloat qyqy = q[SG_Y] * q[SG_Y] ;
1697 sgdFloat qyqz = q[SG_Y] * q[SG_Z] ;
1698 sgdFloat qzqz = q[SG_Z] * q[SG_Z] ;
1699 rot[SG_X] = ( qwqw + qxqx - qyqy - qzqz ) * vec[SG_X] + 2.0f * ( qxqy - qwqz ) * vec[SG_Y] + 2.0f * ( qxqz + qwqy ) * vec[SG_Z] ;
1700 rot[SG_Y] = ( qwqw - qxqx + qyqy - qzqz ) * vec[SG_Y] + 2.0f * ( qyqz - qwqx ) * vec[SG_Z] + 2.0f * ( qxqy + qwqz ) * vec[SG_X] ;
1701 rot[SG_Z] = ( qwqw - qxqx - qyqy + qzqz ) * vec[SG_Z] + 2.0f * ( qxqz - qwqy ) * vec[SG_X] + 2.0f * ( qyqz + qwqx ) * vec[SG_Y] ;
1702 sgdCopyVec3 ( vec, rot ) ;
1703 }
1704
1705 /* Function to rotate a vector through a given quaternion using the formula
1706 * R = Q-1 r Q -- this gives the components of a STATIONARY vector in a ROTATED
1707 * coordinate system. We assume that Q is a unit quaternion.
1708 */
sgdRotateCoordQuat(sgdVec3 vec,sgdQuat q)1709 void sgdRotateCoordQuat ( sgdVec3 vec, sgdQuat q )
1710 {
1711 sgdVec3 rot ;
1712 sgdFloat qwqw = q[SG_W] * q[SG_W] ;
1713 sgdFloat qwqx = q[SG_W] * q[SG_X] ;
1714 sgdFloat qwqy = q[SG_W] * q[SG_Y] ;
1715 sgdFloat qwqz = q[SG_W] * q[SG_Z] ;
1716 sgdFloat qxqx = q[SG_X] * q[SG_X] ;
1717 sgdFloat qxqy = q[SG_X] * q[SG_Y] ;
1718 sgdFloat qxqz = q[SG_X] * q[SG_Z] ;
1719 sgdFloat qyqy = q[SG_Y] * q[SG_Y] ;
1720 sgdFloat qyqz = q[SG_Y] * q[SG_Z] ;
1721 sgdFloat qzqz = q[SG_Z] * q[SG_Z] ;
1722 rot[SG_X] = ( qwqw + qxqx - qyqy - qzqz ) * vec[SG_X] + 2.0f * ( qxqy + qwqz ) * vec[SG_Y] + 2.0f * ( qxqz - qwqy ) * vec[SG_Z] ;
1723 rot[SG_Y] = ( qwqw - qxqx + qyqy - qzqz ) * vec[SG_Y] + 2.0f * ( qyqz + qwqx ) * vec[SG_Z] + 2.0f * ( qxqy - qwqz ) * vec[SG_X] ;
1724 rot[SG_Z] = ( qwqw - qxqx - qyqy + qzqz ) * vec[SG_Z] + 2.0f * ( qxqz + qwqy ) * vec[SG_X] + 2.0f * ( qyqz - qwqx ) * vec[SG_Y] ;
1725 sgdCopyVec3 ( vec, rot ) ;
1726 }
1727
1728
sgdDistSquaredToLineLineSegment(const sgdLineSegment3 seg,const sgdLine3 line)1729 sgdFloat sgdDistSquaredToLineLineSegment ( const sgdLineSegment3 seg, const sgdLine3 line )
1730 {
1731 /* Convert the line segment to a line. We will deal with the segment limits later. */
1732 sgdLine3 line2 ;
1733 sgdLineSegment3ToLine3 ( &line2, seg ) ;
1734
1735 /* Get the dot product of the two direction vectors */
1736 sgdFloat t1_dot_t2 = sgdScalarProductVec3 ( line.direction_vector, line2.direction_vector ) ;
1737
1738 /* If the lines are parallel, distance is just the distance from a point to the other line */
1739 if ( fabs ( t1_dot_t2 ) >= 1.0 )
1740 return sgdDistSquaredToLineVec3 ( line, seg.a ) ;
1741
1742 /* Get the parametric coordinates of the closest points on the two lines. The first line
1743 * is parameterized by r = r1 + t1 u while the second is parameterized by r = r2 + t2 v.
1744 * The square of the distance between them is [ ( r1 + t1 u ) - ( r2 + t2 v ) ] dot
1745 * [ ( r1 + t1 u ) - ( r2 + t2 v ) ]. Differentiating this dot product with respect to
1746 * u and v and setting the derivatives to zero gives a matrix equation:
1747 * [ 1 -(t1 dot t2) ] [ u ] = [ ( r1 - r2 ) dot t1 ]
1748 * [ -(t1 dot t2) 1 ] [ v ] [ -( r1 - r2 ) dot t2 ]
1749 * We invert the matrix to get the equations below.
1750 */
1751 sgdVec3 r1_minus_r2 ;
1752 sgdSubVec3 ( r1_minus_r2, line.point_on_line, line2.point_on_line ) ;
1753
1754 /* t1_t2_t2_minus_t1 = ( t1 dot t2 ) t2 - t1
1755 * t2_minus_t1_t2_t1 = t2 - ( t1 dot t2 ) t1
1756 */
1757 sgdVec3 t1_t2_t2_minus_t1, t2_minus_t1_t2_t1 ;
1758 sgdAddScaledVec3 ( t1_t2_t2_minus_t1, line.direction_vector, line2.direction_vector, -t1_dot_t2 ) ;
1759 sgdNegateVec3 ( t1_t2_t2_minus_t1 ) ;
1760 sgdAddScaledVec3 ( t2_minus_t1_t2_t1, line2.direction_vector, line.direction_vector, -t1_dot_t2 ) ;
1761
1762 sgdFloat u = sgdScalarProductVec3 ( r1_minus_r2, t1_t2_t2_minus_t1 ) / ( 1.0f - t1_dot_t2 * t1_dot_t2 ) ;
1763 sgdFloat v = sgdScalarProductVec3 ( r1_minus_r2, t2_minus_t1_t2_t1 ) / ( 1.0f - t1_dot_t2 * t1_dot_t2 ) ;
1764
1765 /* Since line 2 is a line segment, we limit "v" to between 0 and the distance between the points. */
1766 sgdFloat length = sgdDistanceVec3 ( seg.a, seg.b ) ;
1767 if ( v < 0.0 ) v = 0.0 ;
1768 if ( v > length ) v = length ;
1769
1770 sgdVec3 point1, point2 ;
1771 sgdAddScaledVec3 ( point1, line.point_on_line, line.direction_vector, u ) ;
1772 sgdAddScaledVec3 ( point2, line2.point_on_line, line2.direction_vector, v ) ;
1773 return sgdDistanceSquaredVec3 ( point1, point2 ) ;
1774 }
1775
1776
1777
sgdReflectInPlaneVec3(sgdVec3 dst,const sgdVec3 src,const sgdVec4 plane)1778 void sgdReflectInPlaneVec3 ( sgdVec3 dst, const sgdVec3 src, const sgdVec4 plane )
1779 {
1780 SGDfloat src_dot_norm = sgdScalarProductVec3 ( src, plane ) ;
1781
1782 sgdVec3 tmp ;
1783
1784 sgdScaleVec3 ( tmp, plane, SGD_TWO * src_dot_norm ) ;
1785 sgdSubVec3 ( dst, src, tmp ) ;
1786 }
1787
1788
1789
sgdClassifyMat4(const sgdMat4 m)1790 int sgdClassifyMat4 ( const sgdMat4 m )
1791 {
1792 const SGDfloat epsilon = 1e-6 ;
1793
1794 int flags = 0 ;
1795
1796
1797 SGDfloat sx, sy, sz ;
1798
1799 if ( m[0][1] == SGD_ZERO && m[0][2] == SGD_ZERO &&
1800 m[1][0] == SGD_ZERO && m[1][2] == SGD_ZERO &&
1801 m[2][0] == SGD_ZERO && m[2][1] == SGD_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 ( sgdAbs ( sgdScalarProductVec3 ( m[1], m[2] ) ) > epsilon ||
1823 sgdAbs ( sgdScalarProductVec3 ( m[2], m[0] ) ) > epsilon ||
1824 sgdAbs ( sgdScalarProductVec3 ( m[0], m[1] ) ) > epsilon )
1825 {
1826 flags |= SG_NONORTHO ;
1827 }
1828
1829 sgdVec3 temp ;
1830 sgdVectorProductVec3 ( temp, m[0], m[1] ) ;
1831 SGDfloat det = sgdScalarProductVec3 ( temp, m[2] ) ;
1832
1833 if ( det < 0 )
1834 flags |= SG_MIRROR ;
1835
1836 sx = sgdScalarProductVec3 ( m[0], m[0] ) ;
1837 sy = sgdScalarProductVec3 ( m[1], m[1] ) ;
1838 sz = sgdScalarProductVec3 ( m[2], m[2] ) ;
1839
1840 }
1841
1842
1843 if ( sgdAbs ( sx - sy ) > epsilon ||
1844 sgdAbs ( 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 ( sgdAbs ( sx - SGD_ONE ) > epsilon )
1852 flags |= SG_SCALE ;
1853 }
1854
1855
1856 if ( m[3][0] != SGD_ZERO || m[3][1] != SGD_ZERO || m[3][2] != SGD_ZERO )
1857 {
1858 flags |= SG_TRANSLATION ;
1859 }
1860
1861
1862 if ( m[0][3] != SGD_ZERO || m[1][3] != SGD_ZERO || m[2][3] != SGD_ZERO ||
1863 m[3][3] != SGD_ONE )
1864 {
1865 flags |= SG_PROJECTION ;
1866 }
1867
1868
1869 return flags ;
1870 }
1871
1872
sgdTriangleSolver_ASAtoArea(SGDfloat angA,SGDfloat lenB,SGDfloat angC)1873 SGDfloat sgdTriangleSolver_ASAtoArea ( SGDfloat angA, SGDfloat lenB, SGDfloat angC )
1874 {
1875 /* Get the third angle */
1876
1877 SGDfloat angB = SGD_180 - (angA + angC) ;
1878
1879 /* Use Sine Rule to get length of a second side - then use SAStoArea. */
1880
1881 SGDfloat sinB = sgdSin ( angB ) ;
1882
1883 if ( sinB == SGD_ZERO )
1884 return SGD_ZERO ;
1885
1886 SGDfloat lenA = lenB * sgdSin(angA) / sinB ;
1887
1888 return sgdTriangleSolver_SAStoArea ( lenA, angC, lenB ) ;
1889 }
1890
1891
sgdTriangleSolver_SAStoArea(SGDfloat lenA,SGDfloat angB,SGDfloat lenC)1892 SGDfloat sgdTriangleSolver_SAStoArea ( SGDfloat lenA, SGDfloat angB, SGDfloat lenC )
1893 {
1894 return SGD_HALF * lenC * lenA * sgdSin ( angB ) ;
1895 }
1896
1897
sgdTriangleSolver_SSStoArea(SGDfloat lenA,SGDfloat lenB,SGDfloat lenC)1898 SGDfloat sgdTriangleSolver_SSStoArea ( SGDfloat lenA, SGDfloat lenB, SGDfloat lenC )
1899 {
1900 /* Heron's formula */
1901
1902 SGDfloat s = ( lenA + lenB + lenC ) / SGD_TWO ;
1903 SGDfloat q = s * (s-lenA) * (s-lenB) * (s-lenC) ;
1904
1905 /* Ikky illegal triangles generate zero areas. */
1906
1907 return ( q <= SGD_ZERO ) ? SGD_ZERO : sgdSqrt ( q ) ;
1908 }
1909
1910
sgdTriangleSolver_ASStoArea(SGDfloat angB,SGDfloat lenA,SGDfloat lenB,int angA_is_obtuse)1911 SGDfloat sgdTriangleSolver_ASStoArea ( SGDfloat angB, SGDfloat lenA, SGDfloat lenB,
1912 int angA_is_obtuse )
1913 {
1914 SGDfloat lenC ;
1915
1916 sgdTriangleSolver_ASStoSAA ( angB, lenA, lenB, angA_is_obtuse,
1917 &lenC, NULL, NULL ) ;
1918
1919 return sgdTriangleSolver_SAStoArea ( lenA, angB, lenC ) ;
1920 }
1921
sgdTriangleSolver_SAAtoArea(SGDfloat lenA,SGDfloat angB,SGDfloat angA)1922 SGDfloat sgdTriangleSolver_SAAtoArea ( SGDfloat lenA, SGDfloat angB, SGDfloat angA )
1923 {
1924 SGDfloat lenC ;
1925
1926 sgdTriangleSolver_SAAtoASS ( lenA, angB, angA, NULL, NULL, &lenC ) ;
1927
1928 return sgdTriangleSolver_SAStoArea ( lenA, angB, lenC ) ;
1929 }
1930
sgdTriangleSolver_SSStoAAA(SGDfloat lenA,SGDfloat lenB,SGDfloat lenC,SGDfloat * angA,SGDfloat * angB,SGDfloat * angC)1931 void sgdTriangleSolver_SSStoAAA ( SGDfloat lenA, SGDfloat lenB, SGDfloat lenC,
1932 SGDfloat *angA, SGDfloat *angB, SGDfloat *angC )
1933 {
1934 SGDfloat aa, bb, cc ;
1935
1936 int flag = ( lenA == SGD_ZERO ) |
1937 (( lenB == SGD_ZERO )<<1) |
1938 (( lenC == SGD_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 = sgdACos (( lenB*lenB + lenC*lenC - lenA*lenA )/(SGD_TWO*lenB*lenC)) ;
1949 bb = sgdACos (( lenA*lenA + lenC*lenC - lenB*lenB )/(SGD_TWO*lenA*lenC)) ;
1950 cc = sgdACos (( lenA*lenA + lenB*lenB - lenC*lenC )/(SGD_TWO*lenA*lenB)) ;
1951 break ;
1952
1953 case 1 : /* lenA is zero */
1954 aa = SGD_ZERO ;
1955 bb = cc = SGD_90 ;
1956 break ;
1957
1958 case 2 : /* lenB is zero */
1959 bb = SGD_ZERO ;
1960 aa = cc = SGD_90 ;
1961 break ;
1962
1963 case 4 : /* lenC is zero */
1964 cc = SGD_ZERO ;
1965 aa = bb = SGD_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 = SGD_ZERO ;
1972 break ;
1973
1974 default : /* All three sides are zero length */
1975 aa = bb = cc = SGD_60 ;
1976 break ;
1977 }
1978
1979 if ( angA ) *angA = aa ;
1980 if ( angB ) *angB = bb ;
1981 if ( angC ) *angC = cc ;
1982 }
1983
sgdTriangleSolver_SAStoASA(SGDfloat lenA,SGDfloat angB,SGDfloat lenC,SGDfloat * angA,SGDfloat * lenB,SGDfloat * angC)1984 void sgdTriangleSolver_SAStoASA ( SGDfloat lenA, SGDfloat angB, SGDfloat lenC,
1985 SGDfloat *angA, SGDfloat *lenB, SGDfloat *angC )
1986 {
1987 /* Get third side using Cosine Rule */
1988
1989 SGDfloat s = lenC * lenC +
1990 lenA * lenA - SGD_TWO * lenC * lenA * sgdCos( angB ) ;
1991
1992 SGDfloat lb = ( s <= SGD_ZERO ) ? SGD_ZERO : (SGDfloat) sqrt ( s ) ;
1993
1994 if ( lenB ) *lenB = lb ;
1995
1996 sgdTriangleSolver_SSStoAAA ( lenA, lb, lenC, angA, NULL, angC ) ;
1997 }
1998
1999
sgdTriangleSolver_ASAtoSAS(SGDfloat angA,SGDfloat lenB,SGDfloat angC,SGDfloat * lenA,SGDfloat * angB,SGDfloat * lenC)2000 void sgdTriangleSolver_ASAtoSAS ( SGDfloat angA, SGDfloat lenB, SGDfloat angC,
2001 SGDfloat *lenA, SGDfloat *angB, SGDfloat *lenC )
2002 {
2003 /* Find the missing angle */
2004
2005 SGDfloat bb = SGD_180 - (angA + angC) ;
2006
2007 if ( angB ) *angB = bb ;
2008
2009 /* Use Sine Rule */
2010
2011 SGDfloat sinB = sgdSin ( bb ) ;
2012
2013 if ( sinB == SGD_ZERO )
2014 {
2015 if ( lenA ) *lenA = lenB / SGD_TWO ; /* One valid interpretation */
2016 if ( lenC ) *lenC = lenB / SGD_TWO ;
2017 }
2018 else
2019 {
2020 if ( lenA ) *lenA = lenB * sgdSin(angA) / sinB ;
2021 if ( lenC ) *lenC = lenB * sgdSin(angC) / sinB ;
2022 }
2023 }
2024
2025
sgdTriangleSolver_ASStoSAA(SGDfloat angB,SGDfloat lenA,SGDfloat lenB,int angA_is_obtuse,SGDfloat * lenC,SGDfloat * angA,SGDfloat * angC)2026 void sgdTriangleSolver_ASStoSAA ( SGDfloat angB, SGDfloat lenA, SGDfloat lenB,
2027 int angA_is_obtuse,
2028 SGDfloat *lenC, SGDfloat *angA, SGDfloat *angC )
2029 {
2030 /* Sine law */
2031
2032 SGDfloat aa = (lenB == SGD_ZERO ) ? SGD_ZERO : sgdASin (lenA * sgdSin(angB)/lenB) ;
2033
2034 if ( angA_is_obtuse )
2035 aa = SGD_180 - aa ;
2036
2037 if ( angA ) *angA = aa ;
2038
2039 /* Find the missing angle */
2040
2041 SGDfloat cc = SGD_180 - (aa + angB) ;
2042
2043 if ( angC ) *angC = cc ;
2044
2045 /* Use SAStoASA to get the last length */
2046
2047 sgdTriangleSolver_SAStoASA ( lenA, cc, lenB, NULL, lenC, NULL ) ;
2048 }
2049
2050
sgdTriangleSolver_SAAtoASS(SGDfloat lenA,SGDfloat angB,SGDfloat angA,SGDfloat * angC,SGDfloat * lenB,SGDfloat * lenC)2051 void sgdTriangleSolver_SAAtoASS ( SGDfloat lenA, SGDfloat angB, SGDfloat angA,
2052 SGDfloat *angC, SGDfloat *lenB, SGDfloat *lenC )
2053 {
2054 /* Find the missing angle */
2055
2056 SGDfloat cc = SGD_180 - (angB + angA) ;
2057
2058 if ( angC ) *angC = cc ;
2059
2060 sgdTriangleSolver_ASAtoSAS ( cc, lenA, angB, lenC, NULL, lenB ) ;
2061 }
2062
2063
2064
2065