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.h 2121 2007-09-15 03:36:21Z fayjf $
22 */
23
24
25 #ifndef SG_H
26 #define SG_H 1
27
28 #include <stdio.h>
29 #include "ul.h"
30
31 #define sgFloat float
32 #define SGfloat float
33
34 #define SG_ZERO 0.0f
35 #define SG_HALF 0.5f
36 #define SG_ONE 1.0f
37 #define SG_TWO 2.0f
38 #define SG_THREE 3.0f
39 #define SG_FOUR 4.0f
40 #define SG_45 45.0f
41 #define SG_60 60.0f
42 #define SG_90 90.0f
43 #define SG_180 180.0f
44 #define SG_MAX FLT_MAX
45
46 #define SG_X 0
47 #define SG_Y 1
48 #define SG_Z 2
49 #define SG_W 3
50
51 #ifndef M_PI
52 #define SG_PI 3.1415926535f
53 #else
54 #define SG_PI ((SGfloat) M_PI)
55 #endif
56
57 #define SG_DEGREES_TO_RADIANS (SG_PI/SG_180)
58 #define SG_RADIANS_TO_DEGREES (SG_180/SG_PI)
59
60 /*
61 These are just convenient redefinitions of standard
62 math library functions to stop float/double warnings.
63 */
64
sgSqrt(const SGfloat x)65 inline SGfloat sgSqrt ( const SGfloat x ) { return (SGfloat) sqrt ( x ) ; }
sgSquare(const SGfloat x)66 inline SGfloat sgSquare ( const SGfloat x ) { return x * x ; }
sgAbs(const SGfloat a)67 inline SGfloat sgAbs ( const SGfloat a ) { return (a<SG_ZERO) ? -a : a ; }
sgHypot(const SGfloat x,const SGfloat y)68 inline SGfloat sgHypot ( const SGfloat x, const SGfloat y ) { return sgSqrt( sgSquare(x) + sgSquare(y) ); }
69
70
71 /*
72 Type-casted sin/cos/tan/asin/acos/atan2 ANGLES IN DEGREES
73 */
74
sgASin(SGfloat s)75 inline SGfloat sgASin ( SGfloat s )
76 { return (SGfloat) asin ( s ) * SG_RADIANS_TO_DEGREES ; }
sgACos(SGfloat s)77 inline SGfloat sgACos ( SGfloat s )
78 { return (SGfloat) acos ( s ) * SG_RADIANS_TO_DEGREES ; }
sgATan(SGfloat s)79 inline SGfloat sgATan ( SGfloat s )
80 { return (SGfloat) atan ( s ) * SG_RADIANS_TO_DEGREES ; }
sgATan2(SGfloat y,SGfloat x)81 inline SGfloat sgATan2 ( SGfloat y, SGfloat x )
82 { return (SGfloat) atan2 ( y,x ) * SG_RADIANS_TO_DEGREES ; }
sgSin(SGfloat s)83 inline SGfloat sgSin ( SGfloat s )
84 { return (SGfloat) sin ( s * SG_DEGREES_TO_RADIANS ) ; }
sgCos(SGfloat s)85 inline SGfloat sgCos ( SGfloat s )
86 { return (SGfloat) cos ( s * SG_DEGREES_TO_RADIANS ) ; }
sgTan(SGfloat s)87 inline SGfloat sgTan ( SGfloat s )
88 { return (SGfloat) tan ( s * SG_DEGREES_TO_RADIANS ) ; }
89
90
sgCompareFloat(const SGfloat a,const SGfloat b,const SGfloat tol)91 inline int sgCompareFloat ( const SGfloat a, const SGfloat b, const SGfloat tol )
92 {
93 if ( ( a + tol ) < b ) return -1 ;
94 if ( ( b + tol ) < a ) return 1 ;
95 return 0 ;
96 }
97
98
99 /*
100 Types used in SG.
101 */
102
103 typedef SGfloat sgVec2 [ 2 ] ;
104 typedef SGfloat sgVec3 [ 3 ] ;
105 typedef SGfloat sgVec4 [ 4 ] ;
106
107 typedef sgVec4 sgQuat ;
108
109 typedef SGfloat sgMat3 [3][3] ;
110 typedef SGfloat sgMat4 [4][4] ;
111
112 struct sgCoord
113 {
114 sgVec3 xyz ;
115 sgVec3 hpr ;
116 } ;
117
118 class sgSphere ;
119 class sgBox ;
120 class sgFrustum ;
121
122 /*
123 Some handy constants
124 */
125
126 #define SG_OUTSIDE FALSE
127 #define SG_INSIDE TRUE
128 #define SG_STRADDLE 2
129
sgHeightOfPlaneVec2(const sgVec4 plane,const sgVec2 pnt)130 inline SGfloat sgHeightOfPlaneVec2 ( const sgVec4 plane, const sgVec2 pnt )
131 {
132 if ( plane[2] == SG_ZERO )
133 return SG_ZERO ;
134 else
135 return -( plane[0] * pnt[0] + plane[1] * pnt[1] + plane[3] ) / plane[2] ;
136 }
137
138 /*
139 Convert a direction vector into a set of euler angles,
140 (with zero roll)
141 The direction vector is X-coordinate = north, Y-coordinate = east, Z-coordinate = down
142 The heading ( = hpr[0] ) is in degrees south of east -- this is VERY nonstandard
143 The pitch ( = hpr[1] ) is in degrees positive up
144 The roll ( = hpr[2] ) is zero
145 */
146
147 extern void sgHPRfromVec3 ( sgVec3 hpr, const sgVec3 src ) ;
148
149 extern void sgMakeCoordMat4 ( sgMat4 dst, const SGfloat x, const SGfloat y, const SGfloat z,
150 const SGfloat h, const SGfloat p, const SGfloat r ) ;
151
sgMakeCoordMat4(sgMat4 dst,const sgVec3 xyz,const sgVec3 hpr)152 inline void sgMakeCoordMat4( sgMat4 dst, const sgVec3 xyz, const sgVec3 hpr )
153 {
154 sgMakeCoordMat4 ( dst, xyz[0], xyz[1], xyz[2],
155 hpr[0], hpr[1], hpr[2] ) ;
156 }
157
sgMakeCoordMat4(sgMat4 dst,const sgCoord * src)158 inline void sgMakeCoordMat4( sgMat4 dst, const sgCoord *src )
159 {
160 sgMakeCoordMat4 ( dst, src->xyz, src->hpr ) ;
161 }
162
163 extern void sgMakeLookAtMat4 ( sgMat4 dst,
164 const sgVec3 eye, const sgVec3 center, const sgVec3 up ) ;
165
166 extern void sgMakeRotMat4 ( sgMat4 dst, const SGfloat angle, const sgVec3 axis ) ;
167
sgMakeRotMat4(sgMat4 dst,const sgVec3 hpr)168 inline void sgMakeRotMat4 ( sgMat4 dst, const sgVec3 hpr )
169 {
170 sgMakeCoordMat4 ( dst, SG_ZERO, SG_ZERO, SG_ZERO, hpr[0], hpr[1], hpr[2] ) ;
171 }
172
sgMakeRotMat4(sgMat4 dst,const SGfloat h,const SGfloat p,const SGfloat r)173 inline void sgMakeRotMat4 ( sgMat4 dst,const SGfloat h, const SGfloat p, const SGfloat r )
174 {
175 sgMakeCoordMat4 ( dst, SG_ZERO, SG_ZERO, SG_ZERO, h, p, r ) ;
176 }
177
178 extern void sgMakeTransMat4 ( sgMat4 dst, const sgVec3 xyz ) ;
179 extern void sgMakeTransMat4 ( sgMat4 dst, const SGfloat x, const SGfloat y, const SGfloat z ) ;
180
181
182 extern void sgSetCoord ( sgCoord *coord, const sgMat4 src ) ;
183
184 extern void sgMultMat4 ( sgMat4 dst, const sgMat4 a, const sgMat4 b ) ;
185 extern void sgPostMultMat4 ( sgMat4 dst, const sgMat4 a ) ;
186 extern void sgPreMultMat4 ( sgMat4 dst, const sgMat4 a ) ;
187
188 extern void sgTransposeNegateMat4 ( sgMat4 dst ) ;
189 extern void sgTransposeNegateMat4 ( sgMat4 dst, const sgMat4 src ) ;
190 extern void sgInvertMat4 ( sgMat4 dst, const sgMat4 src ) ;
sgInvertMat4(sgMat4 dst)191 inline void sgInvertMat4 ( sgMat4 dst ) { sgInvertMat4 ( dst, dst ) ; }
192
193 extern void sgXformVec3 ( sgVec3 dst, const sgVec3 src, const sgMat4 mat ) ;
194 extern void sgXformPnt3 ( sgVec3 dst, const sgVec3 src, const sgMat4 mat ) ;
195 extern void sgXformVec4 ( sgVec4 dst, const sgVec4 src, const sgMat4 mat ) ;
196 extern void sgXformPnt4 ( sgVec4 dst, const sgVec4 src, const sgMat4 mat ) ;
197 extern void sgFullXformPnt3 ( sgVec3 dst, const sgVec3 src, const sgMat4 mat ) ;
198 extern void sgFullXformPnt4 ( sgVec4 dst, const sgVec4 src, const sgMat4 mat ) ;
199
sgXformVec3(sgVec3 dst,const sgMat4 mat)200 inline void sgXformVec3 ( sgVec3 dst, const sgMat4 mat ) { sgXformVec3 ( dst, dst, mat ) ; }
sgXformPnt3(sgVec3 dst,const sgMat4 mat)201 inline void sgXformPnt3 ( sgVec3 dst, const sgMat4 mat ) { sgXformPnt3 ( dst, dst, mat ) ; }
sgXformVec4(sgVec4 dst,const sgMat4 mat)202 inline void sgXformVec4 ( sgVec4 dst, const sgMat4 mat ) { sgXformVec4 ( dst, dst, mat ) ; }
sgXformPnt4(sgVec4 dst,const sgMat4 mat)203 inline void sgXformPnt4 ( sgVec4 dst, const sgMat4 mat ) { sgXformPnt4 ( dst, dst, mat ) ; }
sgFullXformPnt3(sgVec3 dst,const sgMat4 mat)204 inline void sgFullXformPnt3 ( sgVec3 dst, const sgMat4 mat ) { sgFullXformPnt3 ( dst, dst, mat ) ; }
sgFullXformPnt4(sgVec4 dst,const sgMat4 mat)205 inline void sgFullXformPnt4 ( sgVec4 dst, const sgMat4 mat ) { sgFullXformPnt4 ( dst, dst, mat ) ; }
206
207
208 /* Bits returned by sgClassifyMat4 */
209
210 #define SG_IDENTITY 0x00 // for clarity
211 #define SG_ROTATION 0x01 // includes a rotational component
212 #define SG_MIRROR 0x02 // changes handedness (det < 0)
213 #define SG_SCALE 0x04 // uniform scaling
214 #define SG_NONORTHO 0x10 // 3x3 not orthogonal
215 #define SG_TRANSLATION 0x20 // translates
216 #define SG_PROJECTION 0x40 // forth column not 0,0,0,1
217
218 /* Are these needed? sgClassifyMat4() does set the general scale bit for some matrices,
219 * but it is not easily defined. Use SG_NONORTHO instead (which is also set). */
220 #define SG_UNIFORM_SCALE SG_SCALE
221 #define SG_GENERAL_SCALE 0x08 // x, y and z scaled differently
222
223 extern int sgClassifyMat4 ( const sgMat4 mat ) ;
224
225
226
227 /*
228 Basic low-level vector functions.
229 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
230 For each of Vec2, Vec3 and Vec4, we provide inlines for
231
232 Zero - set all elements to zero.
233 Set - set each element individually.
234 Add - add vectors element by element.
235 Sub - subtract vectors element by element.
236 Scale - multiply each element of a vector by a variable.
237 AddScaled - multiply second vector by a constant and add the
238 result to the first vector.
239 Negate - negate each element of a vector.
240 Compare - compare vectors element by element with optional tolerance.
241 (return TRUE if vectors are equal - within tolerances)
242 Equal - return TRUE if vectors are exactly equal.
243 Length - compute length of a vector.
244 Distance - compute distance between two points.
245 ScalarProduct - scalar (dot) product.
246 VectorProduct - vector (cross) product (3-element vectors ONLY!).
247 Normalise/Normalize - make vector be one unit long.
248 */
249
sgZeroVec2(sgVec2 dst)250 inline void sgZeroVec2 ( sgVec2 dst ) { dst[0]=dst[1]=SG_ZERO ; }
sgZeroVec3(sgVec3 dst)251 inline void sgZeroVec3 ( sgVec3 dst ) { dst[0]=dst[1]=dst[2]=SG_ZERO ; }
sgZeroVec4(sgVec4 dst)252 inline void sgZeroVec4 ( sgVec4 dst ) { dst[0]=dst[1]=dst[2]=dst[3]=SG_ZERO ; }
253
254
sgSetVec2(sgVec2 dst,const SGfloat x,const SGfloat y)255 inline void sgSetVec2 ( sgVec2 dst, const SGfloat x, const SGfloat y )
256 {
257 dst [ 0 ] = x ;
258 dst [ 1 ] = y ;
259 }
260
sgSetVec3(sgVec3 dst,const SGfloat x,const SGfloat y,const SGfloat z)261 inline void sgSetVec3 ( sgVec3 dst, const SGfloat x, const SGfloat y, const SGfloat z )
262 {
263 dst [ 0 ] = x ;
264 dst [ 1 ] = y ;
265 dst [ 2 ] = z ;
266 }
267
sgSetVec4(sgVec4 dst,const SGfloat x,const SGfloat y,const SGfloat z,const SGfloat w)268 inline void sgSetVec4 ( sgVec4 dst, const SGfloat x, const SGfloat y, const SGfloat z, const SGfloat w )
269 {
270 dst [ 0 ] = x ;
271 dst [ 1 ] = y ;
272 dst [ 2 ] = z ;
273 dst [ 3 ] = w ;
274 }
275
276
sgCopyVec2(sgVec2 dst,const sgVec2 src)277 inline void sgCopyVec2 ( sgVec2 dst, const sgVec2 src )
278 {
279 dst [ 0 ] = src [ 0 ] ;
280 dst [ 1 ] = src [ 1 ] ;
281 }
282
sgCopyVec3(sgVec3 dst,const sgVec3 src)283 inline void sgCopyVec3 ( sgVec3 dst, const sgVec3 src )
284 {
285 dst [ 0 ] = src [ 0 ] ;
286 dst [ 1 ] = src [ 1 ] ;
287 dst [ 2 ] = src [ 2 ] ;
288 }
289
sgCopyVec4(sgVec4 dst,const sgVec4 src)290 inline void sgCopyVec4 ( sgVec4 dst, const sgVec4 src )
291 {
292 dst [ 0 ] = src [ 0 ] ;
293 dst [ 1 ] = src [ 1 ] ;
294 dst [ 2 ] = src [ 2 ] ;
295 dst [ 3 ] = src [ 3 ] ;
296 }
297
298
sgAddVec2(sgVec2 dst,const sgVec2 src)299 inline void sgAddVec2 ( sgVec2 dst, const sgVec2 src )
300 {
301 dst [ 0 ] += src [ 0 ] ;
302 dst [ 1 ] += src [ 1 ] ;
303 }
304
sgAddVec3(sgVec3 dst,const sgVec3 src)305 inline void sgAddVec3 ( sgVec3 dst, const sgVec3 src )
306 {
307 dst [ 0 ] += src [ 0 ] ;
308 dst [ 1 ] += src [ 1 ] ;
309 dst [ 2 ] += src [ 2 ] ;
310 }
311
sgAddVec4(sgVec4 dst,const sgVec4 src)312 inline void sgAddVec4 ( sgVec4 dst, const sgVec4 src )
313 {
314 dst [ 0 ] += src [ 0 ] ;
315 dst [ 1 ] += src [ 1 ] ;
316 dst [ 2 ] += src [ 2 ] ;
317 dst [ 3 ] += src [ 3 ] ;
318 }
319
320
sgAddVec2(sgVec2 dst,const sgVec2 src1,const sgVec2 src2)321 inline void sgAddVec2 ( sgVec2 dst, const sgVec2 src1, const sgVec2 src2 )
322 {
323 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] ;
324 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] ;
325 }
326
sgAddVec3(sgVec3 dst,const sgVec3 src1,const sgVec3 src2)327 inline void sgAddVec3 ( sgVec3 dst, const sgVec3 src1, const sgVec3 src2 )
328 {
329 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] ;
330 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] ;
331 dst [ 2 ] = src1 [ 2 ] + src2 [ 2 ] ;
332 }
333
sgAddVec4(sgVec4 dst,const sgVec4 src1,const sgVec4 src2)334 inline void sgAddVec4 ( sgVec4 dst, const sgVec4 src1, const sgVec4 src2 )
335 {
336 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] ;
337 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] ;
338 dst [ 2 ] = src1 [ 2 ] + src2 [ 2 ] ;
339 dst [ 3 ] = src1 [ 3 ] + src2 [ 3 ] ;
340 }
341
342
sgSubVec2(sgVec2 dst,const sgVec2 src)343 inline void sgSubVec2 ( sgVec2 dst, const sgVec2 src )
344 {
345 dst [ 0 ] -= src [ 0 ] ;
346 dst [ 1 ] -= src [ 1 ] ;
347 }
348
sgSubVec3(sgVec3 dst,const sgVec3 src)349 inline void sgSubVec3 ( sgVec3 dst, const sgVec3 src )
350 {
351 dst [ 0 ] -= src [ 0 ] ;
352 dst [ 1 ] -= src [ 1 ] ;
353 dst [ 2 ] -= src [ 2 ] ;
354 }
355
sgSubVec4(sgVec4 dst,const sgVec4 src)356 inline void sgSubVec4 ( sgVec4 dst, const sgVec4 src )
357 {
358 dst [ 0 ] -= src [ 0 ] ;
359 dst [ 1 ] -= src [ 1 ] ;
360 dst [ 2 ] -= src [ 2 ] ;
361 dst [ 3 ] -= src [ 3 ] ;
362 }
363
sgSubVec2(sgVec2 dst,const sgVec2 src1,const sgVec2 src2)364 inline void sgSubVec2 ( sgVec2 dst, const sgVec2 src1, const sgVec2 src2 )
365 {
366 dst [ 0 ] = src1 [ 0 ] - src2 [ 0 ] ;
367 dst [ 1 ] = src1 [ 1 ] - src2 [ 1 ] ;
368 }
369
sgSubVec3(sgVec3 dst,const sgVec3 src1,const sgVec3 src2)370 inline void sgSubVec3 ( sgVec3 dst, const sgVec3 src1, const sgVec3 src2 )
371 {
372 dst [ 0 ] = src1 [ 0 ] - src2 [ 0 ] ;
373 dst [ 1 ] = src1 [ 1 ] - src2 [ 1 ] ;
374 dst [ 2 ] = src1 [ 2 ] - src2 [ 2 ] ;
375 }
376
sgSubVec4(sgVec4 dst,const sgVec4 src1,const sgVec4 src2)377 inline void sgSubVec4 ( sgVec4 dst, const sgVec4 src1, const sgVec4 src2 )
378 {
379 dst [ 0 ] = src1 [ 0 ] - src2 [ 0 ] ;
380 dst [ 1 ] = src1 [ 1 ] - src2 [ 1 ] ;
381 dst [ 2 ] = src1 [ 2 ] - src2 [ 2 ] ;
382 dst [ 3 ] = src1 [ 3 ] - src2 [ 3 ] ;
383 }
384
385
sgNegateVec2(sgVec2 dst)386 inline void sgNegateVec2 ( sgVec2 dst )
387 {
388 dst [ 0 ] = -dst [ 0 ] ;
389 dst [ 1 ] = -dst [ 1 ] ;
390 }
391
sgNegateVec3(sgVec3 dst)392 inline void sgNegateVec3 ( sgVec3 dst )
393 {
394 dst [ 0 ] = -dst [ 0 ] ;
395 dst [ 1 ] = -dst [ 1 ] ;
396 dst [ 2 ] = -dst [ 2 ] ;
397 }
398
sgNegateVec4(sgVec4 dst)399 inline void sgNegateVec4 ( sgVec4 dst )
400 {
401 dst [ 0 ] = -dst [ 0 ] ;
402 dst [ 1 ] = -dst [ 1 ] ;
403 dst [ 2 ] = -dst [ 2 ] ;
404 dst [ 3 ] = -dst [ 3 ] ;
405 }
406
407
sgNegateVec2(sgVec2 dst,const sgVec2 src)408 inline void sgNegateVec2 ( sgVec2 dst, const sgVec2 src )
409 {
410 dst [ 0 ] = -src [ 0 ] ;
411 dst [ 1 ] = -src [ 1 ] ;
412 }
413
sgNegateVec3(sgVec3 dst,const sgVec3 src)414 inline void sgNegateVec3 ( sgVec3 dst, const sgVec3 src )
415 {
416 dst [ 0 ] = -src [ 0 ] ;
417 dst [ 1 ] = -src [ 1 ] ;
418 dst [ 2 ] = -src [ 2 ] ;
419 }
420
sgNegateVec4(sgVec4 dst,const sgVec4 src)421 inline void sgNegateVec4 ( sgVec4 dst, const sgVec4 src )
422 {
423 dst [ 0 ] = -src [ 0 ] ;
424 dst [ 1 ] = -src [ 1 ] ;
425 dst [ 2 ] = -src [ 2 ] ;
426 dst [ 3 ] = -src [ 3 ] ;
427 }
428
429
sgScaleVec2(sgVec2 dst,const SGfloat s)430 inline void sgScaleVec2 ( sgVec2 dst, const SGfloat s )
431 {
432 dst [ 0 ] *= s ;
433 dst [ 1 ] *= s ;
434 }
435
sgScaleVec3(sgVec3 dst,const SGfloat s)436 inline void sgScaleVec3 ( sgVec3 dst, const SGfloat s )
437 {
438 dst [ 0 ] *= s ;
439 dst [ 1 ] *= s ;
440 dst [ 2 ] *= s ;
441 }
442
sgScaleVec4(sgVec4 dst,const SGfloat s)443 inline void sgScaleVec4 ( sgVec4 dst, const SGfloat s )
444 {
445 dst [ 0 ] *= s ;
446 dst [ 1 ] *= s ;
447 dst [ 2 ] *= s ;
448 dst [ 3 ] *= s ;
449 }
450
sgScaleVec2(sgVec2 dst,const sgVec2 src,const SGfloat s)451 inline void sgScaleVec2 ( sgVec2 dst, const sgVec2 src, const SGfloat s )
452 {
453 dst [ 0 ] = src [ 0 ] * s ;
454 dst [ 1 ] = src [ 1 ] * s ;
455 }
456
sgScaleVec3(sgVec3 dst,const sgVec3 src,const SGfloat s)457 inline void sgScaleVec3 ( sgVec3 dst, const sgVec3 src, const SGfloat s )
458 {
459 dst [ 0 ] = src [ 0 ] * s ;
460 dst [ 1 ] = src [ 1 ] * s ;
461 dst [ 2 ] = src [ 2 ] * s ;
462 }
463
sgScaleVec4(sgVec4 dst,const sgVec4 src,const SGfloat s)464 inline void sgScaleVec4 ( sgVec4 dst, const sgVec4 src, const SGfloat s )
465 {
466 dst [ 0 ] = src [ 0 ] * s ;
467 dst [ 1 ] = src [ 1 ] * s ;
468 dst [ 2 ] = src [ 2 ] * s ;
469 dst [ 3 ] = src [ 3 ] * s ;
470 }
471
472
sgAddScaledVec2(sgVec2 dst,const sgVec2 src,const SGfloat s)473 inline void sgAddScaledVec2 ( sgVec2 dst, const sgVec2 src, const SGfloat s )
474 {
475 dst [ 0 ] += src [ 0 ] * s ;
476 dst [ 1 ] += src [ 1 ] * s ;
477 }
478
sgAddScaledVec3(sgVec3 dst,const sgVec3 src,const SGfloat s)479 inline void sgAddScaledVec3 ( sgVec3 dst, const sgVec3 src, const SGfloat s )
480 {
481 dst [ 0 ] += src [ 0 ] * s ;
482 dst [ 1 ] += src [ 1 ] * s ;
483 dst [ 2 ] += src [ 2 ] * s ;
484 }
485
sgAddScaledVec4(sgVec4 dst,const sgVec4 src,const SGfloat s)486 inline void sgAddScaledVec4 ( sgVec4 dst, const sgVec4 src, const SGfloat s )
487 {
488 dst [ 0 ] += src [ 0 ] * s ;
489 dst [ 1 ] += src [ 1 ] * s ;
490 dst [ 2 ] += src [ 2 ] * s ;
491 dst [ 3 ] += src [ 3 ] * s ;
492 }
493
494
sgAddScaledVec2(sgVec2 dst,const sgVec2 src1,const sgVec2 src2,const SGfloat s)495 inline void sgAddScaledVec2 ( sgVec2 dst, const sgVec2 src1, const sgVec2 src2, const SGfloat s )
496 {
497 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] * s ;
498 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] * s ;
499 }
500
sgAddScaledVec3(sgVec3 dst,const sgVec3 src1,const sgVec3 src2,const SGfloat s)501 inline void sgAddScaledVec3 ( sgVec3 dst, const sgVec3 src1, const sgVec3 src2, const SGfloat s )
502 {
503 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] * s ;
504 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] * s ;
505 dst [ 2 ] = src1 [ 2 ] + src2 [ 2 ] * s ;
506 }
507
sgAddScaledVec4(sgVec4 dst,const sgVec4 src1,const sgVec4 src2,const SGfloat s)508 inline void sgAddScaledVec4 ( sgVec4 dst, const sgVec4 src1, const sgVec4 src2, const SGfloat s )
509 {
510 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] * s ;
511 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] * s ;
512 dst [ 2 ] = src1 [ 2 ] + src2 [ 2 ] * s ;
513 dst [ 3 ] = src1 [ 3 ] + src2 [ 3 ] * s ;
514 }
515
516
sgCompareVec2(const sgVec2 a,const sgVec2 b,const SGfloat tol)517 inline int sgCompareVec2 ( const sgVec2 a, const sgVec2 b, const SGfloat tol )
518 {
519 if ( sgCompareFloat( a[0], b[0], tol ) != 0 ) return FALSE ;
520 if ( sgCompareFloat( a[1], b[1], tol ) != 0 ) return FALSE ;
521
522 return TRUE ;
523 }
524
sgCompareVec3(const sgVec3 a,const sgVec3 b,const SGfloat tol)525 inline int sgCompareVec3 ( const sgVec3 a, const sgVec3 b, const SGfloat tol )
526 {
527 if ( sgCompareFloat( a[0], b[0], tol ) != 0 ) return FALSE ;
528 if ( sgCompareFloat( a[1], b[1], tol ) != 0 ) return FALSE ;
529 if ( sgCompareFloat( a[2], b[2], tol ) != 0 ) return FALSE ;
530
531 return TRUE ;
532 }
533
sgCompareVec4(const sgVec4 a,const sgVec4 b,const SGfloat tol)534 inline int sgCompareVec4 ( const sgVec4 a, const sgVec4 b, const SGfloat tol )
535 {
536 if ( sgCompareFloat( a[0], b[0], tol ) != 0 ) return FALSE ;
537 if ( sgCompareFloat( a[1], b[1], tol ) != 0 ) return FALSE ;
538 if ( sgCompareFloat( a[2], b[2], tol ) != 0 ) return FALSE ;
539 if ( sgCompareFloat( a[3], b[3], tol ) != 0 ) return FALSE ;
540
541 return TRUE ;
542 }
543
544
sgEqualVec2(const sgVec2 a,const sgVec2 b)545 inline int sgEqualVec2 ( const sgVec2 a, const sgVec2 b )
546 {
547 return a[0] == b[0] &&
548 a[1] == b[1] ;
549 }
550
sgEqualVec3(const sgVec3 a,const sgVec3 b)551 inline int sgEqualVec3 ( const sgVec3 a, const sgVec3 b )
552 {
553 return a[0] == b[0] &&
554 a[1] == b[1] &&
555 a[2] == b[2] ;
556 }
557
sgEqualVec4(const sgVec4 a,const sgVec4 b)558 inline int sgEqualVec4 ( const sgVec4 a, const sgVec4 b )
559 {
560 return a[0] == b[0] &&
561 a[1] == b[1] &&
562 a[2] == b[2] &&
563 a[3] == b[3] ;
564 }
565
566
sgScalarProductVec2(const sgVec2 a,const sgVec2 b)567 inline SGfloat sgScalarProductVec2 ( const sgVec2 a, const sgVec2 b )
568 {
569 return a[0]*b[0] + a[1]*b[1] ;
570 }
571
sgScalarProductVec3(const sgVec3 a,const sgVec3 b)572 inline SGfloat sgScalarProductVec3 ( const sgVec3 a, const sgVec3 b )
573 {
574 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] ;
575 }
576
sgScalarProductVec4(const sgVec4 a,const sgVec4 b)577 inline SGfloat sgScalarProductVec4 ( const sgVec4 a, const sgVec4 b )
578 {
579 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3] ;
580 }
581
582
583 extern void sgVectorProductVec3 ( sgVec3 dst, const sgVec3 a, const sgVec3 b ) ;
584
sgLerp(const SGfloat a,const SGfloat b,const SGfloat f)585 inline SGfloat sgLerp ( const SGfloat a, const SGfloat b, const SGfloat f )
586 {
587 return a + f * ( b - a ) ;
588 }
589
sgLerpVec4(sgVec4 dst,const sgVec4 a,const sgVec4 b,const SGfloat f)590 inline void sgLerpVec4 ( sgVec4 dst, const sgVec4 a, const sgVec4 b, const SGfloat f )
591 {
592 dst[0] = a[0] + f * ( b[0] - a[0] ) ;
593 dst[1] = a[1] + f * ( b[1] - a[1] ) ;
594 dst[2] = a[2] + f * ( b[2] - a[2] ) ;
595 dst[3] = a[3] + f * ( b[3] - a[3] ) ;
596 }
597
598
sgLerpVec3(sgVec3 dst,const sgVec3 a,const sgVec3 b,const SGfloat f)599 inline void sgLerpVec3 ( sgVec3 dst, const sgVec3 a, const sgVec3 b, const SGfloat f )
600 {
601 dst[0] = a[0] + f * ( b[0] - a[0] ) ;
602 dst[1] = a[1] + f * ( b[1] - a[1] ) ;
603 dst[2] = a[2] + f * ( b[2] - a[2] ) ;
604 }
605
606
sgLerpVec2(sgVec2 dst,const sgVec2 a,const sgVec2 b,const SGfloat f)607 inline void sgLerpVec2 ( sgVec2 dst, const sgVec2 a, const sgVec2 b, const SGfloat f )
608 {
609 dst[0] = a[0] + f * ( b[0] - a[0] ) ;
610 dst[1] = a[1] + f * ( b[1] - a[1] ) ;
611 }
612
613
sgLerpAnglesVec3(sgVec3 dst,const sgVec3 a,const sgVec3 b,const SGfloat f)614 inline void sgLerpAnglesVec3 ( sgVec3 dst, const sgVec3 a,
615 const sgVec3 b,
616 const SGfloat f )
617 {
618 sgVec3 tmp ;
619
620 if ( b[0] - a[0] > 180.0f ) tmp[0] = a[0] + 360.0f ; else
621 if ( b[0] - a[0] < -180.0f ) tmp[0] = a[0] - 360.0f ; else tmp[0] = a[0] ;
622
623 if ( b[1] - a[1] > 180.0f ) tmp[1] = a[1] + 360.0f ; else
624 if ( b[1] - a[1] < -180.0f ) tmp[1] = a[1] - 360.0f ; else tmp[1] = a[1] ;
625
626 if ( b[2] - a[2] > 180.0f ) tmp[2] = a[2] + 360.0f ; else
627 if ( b[2] - a[2] < -180.0f ) tmp[2] = a[2] - 360.0f ; else tmp[2] = a[2] ;
628
629 dst[0] = tmp[0] + f * ( b[0] - tmp[0] ) ;
630 dst[1] = tmp[1] + f * ( b[1] - tmp[1] ) ;
631 dst[2] = tmp[2] + f * ( b[2] - tmp[2] ) ;
632 }
633
634
635
sgDistanceSquaredVec2(const sgVec2 a,const sgVec2 b)636 inline SGfloat sgDistanceSquaredVec2 ( const sgVec2 a, const sgVec2 b )
637 {
638 return sgSquare ( a[0]-b[0] ) + sgSquare ( a[1]-b[1] ) ;
639 }
640
sgDistanceSquaredVec3(const sgVec3 a,const sgVec3 b)641 inline SGfloat sgDistanceSquaredVec3 ( const sgVec3 a, const sgVec3 b )
642 {
643 return sgSquare ( a[0]-b[0] ) + sgSquare ( a[1]-b[1] ) +
644 sgSquare ( a[2]-b[2] ) ;
645 }
646
sgDistanceSquaredVec4(const sgVec4 a,const sgVec4 b)647 inline SGfloat sgDistanceSquaredVec4 ( const sgVec4 a, const sgVec4 b )
648 {
649 return sgSquare ( a[0]-b[0] ) + sgSquare ( a[1]-b[1] ) +
650 sgSquare ( a[2]-b[2] ) + sgSquare ( a[3]-b[3] ) ;
651 }
652
sgDistanceVec2(const sgVec2 a,const sgVec2 b)653 inline SGfloat sgDistanceVec2 ( const sgVec2 a, const sgVec2 b )
654 {
655 return sgSqrt ( sgSquare ( a[0]-b[0] ) + sgSquare ( a[1]-b[1] ) ) ;
656 }
657
sgDistanceVec3(const sgVec3 a,const sgVec3 b)658 inline SGfloat sgDistanceVec3 ( const sgVec3 a, const sgVec3 b )
659 {
660 return sgSqrt ( sgSquare ( a[0]-b[0] ) + sgSquare ( a[1]-b[1] ) +
661 sgSquare ( a[2]-b[2] ) ) ;
662 }
663
sgDistanceVec4(const sgVec4 a,const sgVec4 b)664 inline SGfloat sgDistanceVec4 ( const sgVec4 a, const sgVec4 b )
665 {
666 return sgSqrt ( sgSquare ( a[0]-b[0] ) + sgSquare ( a[1]-b[1] ) +
667 sgSquare ( a[2]-b[2] ) + sgSquare ( a[3]-b[3] ) ) ;
668 }
669
670
sgLengthVec2(const sgVec2 src)671 inline SGfloat sgLengthVec2 ( const sgVec2 src )
672 {
673 return sgSqrt ( sgScalarProductVec2 ( src, src ) ) ;
674 }
675
sgLengthVec3(const sgVec3 src)676 inline SGfloat sgLengthVec3 ( const sgVec3 src )
677 {
678 return sgSqrt ( sgScalarProductVec3 ( src, src ) ) ;
679 }
680
sgLengthVec4(const sgVec4 src)681 inline SGfloat sgLengthVec4 ( const sgVec4 src )
682 {
683 return sgSqrt ( sgScalarProductVec4 ( src, src ) ) ;
684 }
685
sgLengthSquaredVec2(sgVec2 const src)686 inline SGfloat sgLengthSquaredVec2 ( sgVec2 const src )
687 {
688 return sgScalarProductVec2 ( src, src ) ;
689 }
690
sgLengthSquaredVec3(sgVec3 const src)691 inline SGfloat sgLengthSquaredVec3 ( sgVec3 const src )
692 {
693 return sgScalarProductVec3 ( src, src ) ;
694 }
695
sgLengthSquaredVec4(sgVec4 const src)696 inline SGfloat sgLengthSquaredVec4 ( sgVec4 const src )
697 {
698 return sgScalarProductVec4 ( src, src ) ;
699 }
700
701
702 /* Anglo-US spelling issues. <sigh> */
703 #define sgNormalizeVec2 sgNormaliseVec2
704 #define sgNormalizeVec3 sgNormaliseVec3
705 #define sgNormalizeVec4 sgNormaliseVec4
706 #define sgNormalizeQuat sgNormaliseQuat
707
sgNormaliseVec2(sgVec2 dst)708 inline void sgNormaliseVec2 ( sgVec2 dst )
709 {
710 sgScaleVec2 ( dst, SG_ONE / sgLengthVec2 ( dst ) ) ;
711 }
712
sgNormaliseVec3(sgVec3 dst)713 inline void sgNormaliseVec3 ( sgVec3 dst )
714 {
715 sgScaleVec3 ( dst, SG_ONE / sgLengthVec3 ( dst ) ) ;
716 }
717
sgNormaliseVec4(sgVec4 dst)718 inline void sgNormaliseVec4 ( sgVec4 dst )
719 {
720 sgScaleVec4 ( dst, SG_ONE / sgLengthVec4 ( dst ) ) ;
721 }
722
sgNormaliseVec2(sgVec2 dst,const sgVec2 src)723 inline void sgNormaliseVec2 ( sgVec2 dst, const sgVec2 src )
724 {
725 sgScaleVec2 ( dst, src, SG_ONE / sgLengthVec2 ( src ) ) ;
726 }
727
sgNormaliseVec3(sgVec3 dst,const sgVec3 src)728 inline void sgNormaliseVec3 ( sgVec3 dst, const sgVec3 src )
729 {
730 sgScaleVec3 ( dst, src, SG_ONE / sgLengthVec3 ( src ) ) ;
731 }
732
sgNormaliseVec4(sgVec4 dst,const sgVec4 src)733 inline void sgNormaliseVec4 ( sgVec4 dst, const sgVec4 src )
734 {
735 sgScaleVec4 ( dst, src, SG_ONE / sgLengthVec4 ( src ) ) ;
736 }
737
738
sgZeroCoord(sgCoord * dst)739 inline void sgZeroCoord ( sgCoord *dst )
740 {
741 sgSetVec3 ( dst->xyz, SG_ZERO, SG_ZERO, SG_ZERO ) ;
742 sgSetVec3 ( dst->hpr, SG_ZERO, SG_ZERO, SG_ZERO ) ;
743 }
744
sgSetCoord(sgCoord * dst,const SGfloat x,const SGfloat y,const SGfloat z,const SGfloat h,const SGfloat p,const SGfloat r)745 inline void sgSetCoord ( sgCoord *dst, const SGfloat x, const SGfloat y, const SGfloat z,
746 const SGfloat h, const SGfloat p, const SGfloat r )
747 {
748 sgSetVec3 ( dst->xyz, x, y, z ) ;
749 sgSetVec3 ( dst->hpr, h, p, r ) ;
750 }
751
sgSetCoord(sgCoord * dst,const sgVec3 xyz,const sgVec3 hpr)752 inline void sgSetCoord ( sgCoord *dst, const sgVec3 xyz, const sgVec3 hpr )
753 {
754 sgCopyVec3 ( dst->xyz, xyz ) ;
755 sgCopyVec3 ( dst->hpr, hpr ) ;
756 }
757
sgCopyCoord(sgCoord * dst,const sgCoord * src)758 inline void sgCopyCoord ( sgCoord *dst, const sgCoord *src )
759 {
760 sgCopyVec3 ( dst->xyz, src->xyz ) ;
761 sgCopyVec3 ( dst->hpr, src->hpr ) ;
762 }
763
764
765
sgCopyMat4(sgMat4 dst,const sgMat4 src)766 inline void sgCopyMat4 ( sgMat4 dst, const sgMat4 src )
767 {
768 sgCopyVec4 ( dst[ 0 ], src[ 0 ] ) ;
769 sgCopyVec4 ( dst[ 1 ], src[ 1 ] ) ;
770 sgCopyVec4 ( dst[ 2 ], src[ 2 ] ) ;
771 sgCopyVec4 ( dst[ 3 ], src[ 3 ] ) ;
772 }
773
774
sgScaleMat4(sgMat4 dst,const sgMat4 src,const SGfloat scale)775 inline void sgScaleMat4 ( sgMat4 dst, const sgMat4 src, const SGfloat scale )
776 {
777 sgScaleVec4 ( dst[0], src[0], scale ) ;
778 sgScaleVec4 ( dst[1], src[1], scale ) ;
779 sgScaleVec4 ( dst[2], src[2], scale ) ;
780 sgScaleVec4 ( dst[3], src[3], scale ) ;
781 }
782
783
sgMakeIdentMat4(sgMat4 dst)784 inline void sgMakeIdentMat4 ( sgMat4 dst )
785 {
786 sgSetVec4 ( dst[0], SG_ONE , SG_ZERO, SG_ZERO, SG_ZERO ) ;
787 sgSetVec4 ( dst[1], SG_ZERO, SG_ONE , SG_ZERO, SG_ZERO ) ;
788 sgSetVec4 ( dst[2], SG_ZERO, SG_ZERO, SG_ONE , SG_ZERO ) ;
789 sgSetVec4 ( dst[3], SG_ZERO, SG_ZERO, SG_ZERO, SG_ONE ) ;
790 }
791
792
793 extern void sgMakePickMatrix( sgMat4 mat, sgFloat x, sgFloat y,
794 sgFloat width, sgFloat height, sgVec4 viewport ) ;
795
796 extern int sgCompare3DSqdDist ( const sgVec3 a, const sgVec3 b, const SGfloat sqd_dist ) ;
797
sgDistToLineVec2(const sgVec3 line,const sgVec2 pnt)798 inline SGfloat sgDistToLineVec2 ( const sgVec3 line, const sgVec2 pnt )
799 {
800 return sgScalarProductVec2 ( line, pnt ) + line[2] ;
801 }
802
803
804 struct sgLineSegment3 /* Bounded line segment */
805 {
806 sgVec3 a ;
807 sgVec3 b ;
808 } ;
809
810 struct sgLine3 /* Infinite line */
811 {
812 sgVec3 point_on_line ;
813 sgVec3 direction_vector ; /* Should be a unit vector */
814 } ;
815
816
sgLineSegment3ToLine3(sgLine3 * line,const sgLineSegment3 lineseg)817 inline void sgLineSegment3ToLine3 ( sgLine3 *line,
818 const sgLineSegment3 lineseg )
819 {
820 sgCopyVec3 ( line->point_on_line , lineseg.a ) ;
821 sgSubVec3 ( line->direction_vector, lineseg.b, lineseg.a ) ;
822 sgNormaliseVec3 ( line->direction_vector ) ;
823 }
824
825
826 SGfloat sgDistSquaredToLineVec3 ( const sgLine3 line,
827 const sgVec3 pnt ) ;
828 SGfloat sgDistSquaredToLineSegmentVec3 ( const sgLineSegment3 line,
829 const sgVec3 pnt ) ;
830
831
sgDistToLineVec3(const sgLine3 line,const sgVec3 pnt)832 inline SGfloat sgDistToLineVec3 ( const sgLine3 line,
833 const sgVec3 pnt )
834 {
835 return sgSqrt ( sgDistSquaredToLineVec3 ( line, pnt ) );
836 }
837
838
sgDistToLineSegmentVec3(const sgLineSegment3 line,const sgVec3 pnt)839 inline SGfloat sgDistToLineSegmentVec3 ( const sgLineSegment3 line,
840 const sgVec3 pnt )
841 {
842 return sgSqrt ( sgDistSquaredToLineSegmentVec3(line,pnt) ) ;
843 }
844
845
sgDistToPlaneVec3(const sgVec4 plane,const sgVec3 pnt)846 inline SGfloat sgDistToPlaneVec3 ( const sgVec4 plane, const sgVec3 pnt )
847 {
848 return sgScalarProductVec3 ( plane, pnt ) + plane[3] ;
849 }
850
851
sgHeightAbovePlaneVec3(const sgVec4 plane,const sgVec3 pnt)852 inline SGfloat sgHeightAbovePlaneVec3 ( const sgVec4 plane, const sgVec3 pnt )
853 {
854 return pnt[2] - sgHeightOfPlaneVec2 ( plane, pnt ) ;
855 }
856
857 extern void sgReflectInPlaneVec3 ( sgVec3 dst, const sgVec3 src, const sgVec4 plane ) ;
858
sgReflectInPlaneVec3(sgVec3 dst,const sgVec4 plane)859 inline void sgReflectInPlaneVec3 ( sgVec3 dst, const sgVec4 plane )
860 {
861 sgReflectInPlaneVec3 ( dst, dst, plane ) ;
862 }
863
864 extern void sgMakeNormal ( sgVec2 dst, const sgVec3 a, const sgVec3 b ) ;
865
866
867 extern void sgMakeNormal ( sgVec3 dst, const sgVec3 a, const sgVec3 b, const sgVec3 c ) ;
868
869
sgMake2DLine(sgVec3 dst,const sgVec2 a,const sgVec2 b)870 inline void sgMake2DLine ( sgVec3 dst, const sgVec2 a, const sgVec2 b )
871 {
872 dst[0] = b[1]-a[1] ;
873 dst[1] = -(b[0]-a[0]) ;
874 sgNormalizeVec2 ( dst ) ;
875 dst[2] = - ( dst[0]*a[0] + dst[1]*a[1] ) ;
876 }
877
sgMakePlane(sgVec4 dst,const sgVec3 normal,const sgVec3 pnt)878 inline void sgMakePlane ( sgVec4 dst, const sgVec3 normal, const sgVec3 pnt )
879 {
880 sgCopyVec3 ( dst, normal ) ;
881 dst [ 3 ] = - sgScalarProductVec3 ( normal, pnt ) ;
882 }
883
sgMakePlane(sgVec4 dst,const sgVec3 a,const sgVec3 b,const sgVec3 c)884 inline void sgMakePlane ( sgVec4 dst, const sgVec3 a, const sgVec3 b, const sgVec3 c )
885 {
886 /*
887 Ax + By + Cz + D == 0 ;
888 D = - ( Ax + By + Cz )
889 = - ( A*a[0] + B*a[1] + C*a[2] )
890 = - sgScalarProductVec3 ( normal, a ) ;
891 */
892
893 sgMakeNormal ( dst, a, b, c ) ;
894
895 dst [ 3 ] = - sgScalarProductVec3 ( dst, a ) ;
896 }
897
898 float sgTriArea( sgVec3 p0, sgVec3 p1, sgVec3 p2 );
899
900
901 // Fast code. Result is in the range 0..180:
sgAngleBetweenNormalizedVec3(sgVec3 v1,sgVec3 v2)902 inline SGfloat sgAngleBetweenNormalizedVec3 ( sgVec3 v1, sgVec3 v2 )
903 {
904 float f = sgScalarProductVec3 ( v1, v2 ) ;
905
906 return (float) acos ( ( f >= 1.0f ) ? 1.0f :
907 ( f <= -1.0f ) ? -1.0f : f ) *
908 SG_RADIANS_TO_DEGREES ;
909 }
910
911 // Fast code. Result is in the range 0..180:
912
913 SGfloat sgAngleBetweenVec3 ( sgVec3 v1, sgVec3 v2 );
914
915 // All three have to be normalized. Slow code. Result is in the range 0..360:
916
917 SGfloat sgAngleBetweenNormalizedVec3 (sgVec3 first, sgVec3 second, sgVec3 normal);
918
919 // Normal has to be normalized. Slow code. Result is in the range 0..360:
920
921 SGfloat sgAngleBetweenVec3 ( sgVec3 v1, sgVec3 v2, sgVec3 normal );
922
923 class sgSphere
924 {
925 public:
926 sgVec3 center ;
927 SGfloat radius ;
928
929
sgSphere()930 sgSphere () { empty () ; }
931
getCenter(void)932 const SGfloat *getCenter (void) const { return center ; }
933
setCenter(const sgVec3 c)934 void setCenter ( const sgVec3 c )
935 {
936 sgCopyVec3 ( center, c ) ;
937 }
938
setCenter(const SGfloat x,const SGfloat y,const SGfloat z)939 void setCenter ( const SGfloat x, const SGfloat y, const SGfloat z )
940 {
941 sgSetVec3 ( center, x, y, z ) ;
942 }
943
getRadius(void)944 SGfloat getRadius (void) const { return radius ; }
setRadius(const SGfloat r)945 void setRadius ( const SGfloat r ) { radius = r ; }
946
isEmpty(void)947 int isEmpty (void) const { return radius < SG_ZERO ; }
empty(void)948 void empty (void) { radius = - SG_ONE ; }
949
orthoXform(const sgMat4 m)950 void orthoXform ( const sgMat4 m )
951 {
952 sgXformPnt3 ( center, center, m ) ;
953 // radius *= sgLengthVec3 ( m[0] ) ; -- degrades performance for non-scaled matrices ...
954 }
955
956 void extend ( const sgSphere *s ) ;
957 void extend ( const sgBox *b ) ;
958 void extend ( const sgVec3 v ) ;
959
intersects(const sgSphere * s)960 int intersects ( const sgSphere *s ) const
961 {
962 return sgCompare3DSqdDist ( center, s->getCenter(),
963 sgSquare ( radius + s->getRadius() ) ) <= 0 ;
964 }
965
intersects(const sgVec4 plane)966 int intersects ( const sgVec4 plane ) const
967 {
968 return sgAbs ( sgDistToPlaneVec3 ( plane, center ) ) <= radius ;
969 }
970
971 int intersects ( const sgBox *b ) const ;
972 } ;
973
974
975 class sgBox
976 {
977 public:
978 sgVec3 min ;
979 sgVec3 max ;
980
981
sgBox()982 sgBox () { empty () ; }
983
getMin(void)984 const SGfloat *getMin (void) const { return min ; }
getMax(void)985 const SGfloat *getMax (void) const { return max ; }
986
setMin(const SGfloat x,const SGfloat y,const SGfloat z)987 void setMin ( const SGfloat x, const SGfloat y, const SGfloat z )
988 {
989 sgSetVec3 ( min, x, y, z ) ;
990 }
991
setMin(const sgVec3 src)992 void setMin ( const sgVec3 src )
993 {
994 sgCopyVec3 ( min, src ) ;
995 }
996
setMax(const SGfloat x,const SGfloat y,const SGfloat z)997 void setMax ( const SGfloat x, const SGfloat y, const SGfloat z )
998 {
999 sgSetVec3 ( max, x, y, z ) ;
1000 }
1001
setMax(const sgVec3 src)1002 void setMax ( const sgVec3 src )
1003 {
1004 sgCopyVec3 ( max, src ) ;
1005 }
1006
isEmpty(void)1007 int isEmpty(void) const
1008 {
1009 return ( min[0] > max[0] ||
1010 min[1] > max[1] ||
1011 min[2] > max[2] ) ;
1012 }
1013
empty(void)1014 void empty (void)
1015 {
1016 sgSetVec3 ( min, SG_MAX, SG_MAX, SG_MAX ) ;
1017 sgSetVec3 ( max, -SG_MAX, -SG_MAX, -SG_MAX ) ;
1018 }
1019
1020 void extend ( const sgSphere *s ) ;
1021 void extend ( const sgBox *b ) ;
1022 void extend ( const sgVec3 v ) ;
1023
intersects(const sgSphere * s)1024 int intersects ( const sgSphere *s ) const
1025 {
1026 return s -> intersects ( this ) ;
1027 }
1028
intersects(const sgBox * b)1029 int intersects ( const sgBox *b ) const
1030 {
1031 return min[0] <= b->getMax()[0] && max[0] >= b->getMin()[0] &&
1032 min[1] <= b->getMax()[1] && max[1] >= b->getMin()[1] &&
1033 min[2] <= b->getMax()[2] && max[2] >= b->getMin()[2] ;
1034 }
1035
1036 int intersects ( const sgVec4 plane ) const ;
1037 } ;
1038
1039
1040 #define SG_LEFT_PLANE 0
1041 #define SG_RIGHT_PLANE 1
1042 #define SG_BOT_PLANE 2
1043 #define SG_TOP_PLANE 3
1044 #define SG_NEAR_PLANE 4
1045 #define SG_FAR_PLANE 5
1046
1047 class sgFrustum
1048 {
1049 /* Is the projection orthographic (or perspective)? */
1050 int ortho ;
1051
1052 /* The parameters for glFrustum/glOrtho */
1053
1054 SGfloat left, right, bot, top, nnear, ffar ;
1055
1056 /* The computed projection matrix for this frustum */
1057
1058 sgMat4 mat ;
1059
1060 /* The A,B,C,D terms of the plane equations of the clip planes */
1061 /* A point (x,y,z) is inside the frustum iff Ax + By + Cz + D >= 0 for all planes */
1062
1063 sgVec4 plane [ 6 ] ;
1064
1065 /* These two are only valid for simple frusta */
1066 /* Note that to convert to parameters for gluPerspective you do the following:
1067 * "fovy" is "vfov"
1068 * "aspect" is the ratio "hfov" / "vfov"
1069 * Note also that the "hfov" and "vfov" variables are NOT automatically set.
1070 */
1071
1072 SGfloat hfov ; /* Horizontal Field of View -or- Orthographic Width */
1073 SGfloat vfov ; /* Vertical Field of View -or- Orthographic Height */
1074
1075 void update (void) ;
1076 int getOutcode ( const sgVec4 src ) const ;
1077
1078 public:
1079
sgFrustum(void)1080 sgFrustum (void)
1081 {
1082 ortho = FALSE ;
1083 nnear = SG_ONE ;
1084 ffar = 1000000.0f ;
1085 hfov = SG_45 ;
1086 vfov = SG_45 ;
1087 update () ;
1088 }
1089
setFrustum(const SGfloat l,const SGfloat r,const SGfloat b,const SGfloat t,const SGfloat n,const SGfloat f)1090 void setFrustum ( const SGfloat l, const SGfloat r,
1091 const SGfloat b, const SGfloat t,
1092 const SGfloat n, const SGfloat f )
1093 {
1094 ortho = FALSE ;
1095 left = l ;
1096 right = r ;
1097 bot = b ;
1098 top = t ;
1099 nnear = n ;
1100 ffar = f ;
1101 hfov = vfov = SG_ZERO ;
1102 update () ;
1103 }
1104
setOrtho(const SGfloat l,const SGfloat r,const SGfloat b,const SGfloat t,const SGfloat n,const SGfloat f)1105 void setOrtho ( const SGfloat l, const SGfloat r,
1106 const SGfloat b, const SGfloat t,
1107 const SGfloat n, const SGfloat f )
1108 {
1109 ortho = TRUE ;
1110 left = l ;
1111 right = r ;
1112 bot = b ;
1113 top = t ;
1114 nnear = n ;
1115 ffar = f ;
1116 hfov = vfov = SG_ZERO ;
1117 update () ;
1118 }
1119
getMat4(sgMat4 dst)1120 void getMat4 ( sgMat4 dst ) { sgCopyMat4 ( dst, mat ) ; }
1121
getLeft(void)1122 SGfloat getLeft (void) const { return left ; }
getRight(void)1123 SGfloat getRight(void) const { return right ; }
getBot(void)1124 SGfloat getBot (void) const { return bot ; }
getTop(void)1125 SGfloat getTop (void) const { return top ; }
getNear(void)1126 SGfloat getNear (void) const { return nnear ; }
getFar(void)1127 SGfloat getFar (void) const { return ffar ; }
1128
getPlane(int i)1129 const SGfloat *getPlane ( int i ) const { return plane [ i ] ; }
1130
getHFOV(void)1131 SGfloat getHFOV (void) const { return hfov ; }
getVFOV(void)1132 SGfloat getVFOV (void) const { return vfov ; }
1133
getFOV(SGfloat * h,SGfloat * v)1134 void getFOV ( SGfloat *h, SGfloat *v ) const
1135 {
1136 if ( h != (SGfloat *) 0 ) *h = hfov ;
1137 if ( v != (SGfloat *) 0 ) *v = vfov ;
1138 }
1139
setFOV(const SGfloat h,const SGfloat v)1140 void setFOV ( const SGfloat h, const SGfloat v )
1141 {
1142 ortho = FALSE ;
1143 hfov = ( h <= 0 ) ? ( v * SG_FOUR / SG_THREE ) : h ;
1144 vfov = ( v <= 0 ) ? ( h * SG_THREE / SG_FOUR ) : v ;
1145 update () ;
1146 }
1147
getOrtho(SGfloat * w,SGfloat * h)1148 void getOrtho ( SGfloat *w, SGfloat *h ) const
1149 {
1150 if ( w != (SGfloat *) 0 ) *w = right - left ;
1151 if ( h != (SGfloat *) 0 ) *h = top - bot ;
1152 }
1153
setOrtho(const SGfloat w,const SGfloat h)1154 void setOrtho ( const SGfloat w, const SGfloat h )
1155 {
1156 ortho = TRUE ;
1157 hfov = ( w <= 0 ) ? ( h * SG_FOUR / SG_THREE ) : w ;
1158 vfov = ( h <= 0 ) ? ( w * SG_FOUR / SG_THREE ) : h ;
1159 update () ;
1160 }
1161
getNearFar(SGfloat * n,SGfloat * f)1162 void getNearFar ( SGfloat *n, SGfloat *f ) const
1163 {
1164 if ( n != (SGfloat *) 0 ) *n = nnear ;
1165 if ( f != (SGfloat *) 0 ) *f = ffar ;
1166 }
1167
setNearFar(const SGfloat n,const SGfloat f)1168 void setNearFar ( const SGfloat n, const SGfloat f )
1169 {
1170 nnear = n ;
1171 ffar = f ;
1172 update () ;
1173 }
1174
isOrtho(void)1175 int isOrtho (void) const { return ortho ; }
1176
1177 int contains ( const sgVec3 p ) const ;
1178 int contains ( const sgSphere *s ) const ;
1179 int contains ( const sgBox *b ) const ;
1180 } ;
1181
1182
1183 /*
1184 Quaternion routines are Copyright (C) 1999
1185 Kevin B. Thompson <kevinbthompson@yahoo.com>
1186 Modified by Sylvan W. Clebsch <sylvan@stanford.edu>
1187 Largely rewritten by "Negative0" <negative0@earthlink.net>
1188 Added to by John Fay
1189 */
1190
1191 /*
1192 Quaternion structure w = real, (x, y, z) = vector
1193 CHANGED sqQuat to float array so that syntax matches
1194 vector and matrix routines
1195 */
1196
1197
sgMakeIdentQuat(sgQuat dst)1198 inline void sgMakeIdentQuat ( sgQuat dst )
1199 {
1200 sgSetVec4 ( dst, SG_ZERO, SG_ZERO, SG_ZERO, SG_ONE ) ;
1201 }
1202
1203
sgSetQuat(sgQuat dst,const SGfloat w,const SGfloat x,const SGfloat y,const SGfloat z)1204 inline void sgSetQuat ( sgQuat dst,
1205 const SGfloat w, const SGfloat x,
1206 const SGfloat y, const SGfloat z )
1207 {
1208 sgSetVec4 ( dst, x, y, z, w ) ;
1209 }
1210
sgCopyQuat(sgQuat dst,const sgQuat src)1211 inline void sgCopyQuat ( sgQuat dst, const sgQuat src )
1212 {
1213 sgCopyVec4 ( dst, src ) ;
1214 }
1215
1216
1217 /* Construct a unit quaternion (length==1) */
1218
sgNormaliseQuat(sgQuat dst,const sgQuat src)1219 inline void sgNormaliseQuat ( sgQuat dst, const sgQuat src )
1220 {
1221 SGfloat d = sgScalarProductVec4 ( src, src ) ;
1222
1223 d = (d > SG_ZERO) ? (SG_ONE / sgSqrt ( d )) : SG_ONE ;
1224
1225 sgScaleVec4 ( dst, src, d ) ;
1226 }
1227
1228
1229
sgNormaliseQuat(sgQuat dst)1230 inline void sgNormaliseQuat ( sgQuat dst ) { sgNormaliseQuat ( dst, dst ) ; }
1231
1232
sgInvertQuat(sgQuat dst,const sgQuat src)1233 inline void sgInvertQuat ( sgQuat dst, const sgQuat src )
1234 {
1235 SGfloat d = sgScalarProductVec4 ( src, src ) ;
1236
1237 d = ( d == SG_ZERO ) ? SG_ONE : ( SG_ONE / d ) ;
1238
1239 dst[SG_W] = src[SG_W] * d ;
1240 dst[SG_X] = -src[SG_X] * d ;
1241 dst[SG_Y] = -src[SG_Y] * d ;
1242 dst[SG_Z] = -src[SG_Z] * d ;
1243 }
1244
sgInvertQuat(sgQuat dst)1245 inline void sgInvertQuat ( sgQuat dst ) { sgInvertQuat ( dst, dst ) ; }
1246
1247
1248 /* Make an angle and axis of rotation from a Quaternion. */
1249
1250 void sgQuatToAngleAxis ( SGfloat *angle, sgVec3 axis, const sgQuat src ) ;
1251 void sgQuatToAngleAxis ( SGfloat *angle,
1252 SGfloat *x, SGfloat *y, SGfloat *z,
1253 const sgQuat src ) ;
1254
1255 /* Make a quaternion from a given angle and axis of rotation */
1256
1257 void sgAngleAxisToQuat ( sgQuat dst, const SGfloat angle, const sgVec3 axis ) ;
1258 void sgAngleAxisToQuat ( sgQuat dst,
1259 const SGfloat angle,
1260 const SGfloat x, const SGfloat y, const SGfloat z ) ;
1261
1262 /* Convert a matrix to/from a quat */
1263
1264 void sgMatrixToQuat ( sgQuat quat, const sgMat4 m ) ;
1265 void sgQuatToMatrix ( sgMat4 m, const sgQuat quat ) ;
1266
1267 /* Convert a set of eulers to/from a quat */
1268
1269 void sgQuatToEuler( sgVec3 hpr, const sgQuat quat ) ;
1270 void sgEulerToQuat( sgQuat quat, const sgVec3 hpr ) ;
1271
sgEulerToQuat(sgQuat dst,SGfloat h,SGfloat p,SGfloat r)1272 inline void sgEulerToQuat( sgQuat dst,
1273 SGfloat h, SGfloat p, SGfloat r )
1274 {
1275 sgVec3 hpr ;
1276
1277 sgSetVec3 ( hpr, h, p, r ) ;
1278
1279 sgEulerToQuat( dst, hpr ) ;
1280 }
1281
sgHPRToQuat(sgQuat dst,SGfloat h,SGfloat p,SGfloat r)1282 inline void sgHPRToQuat ( sgQuat dst, SGfloat h, SGfloat p, SGfloat r )
1283 {
1284 sgVec3 hpr;
1285
1286 hpr[0] = h * SG_DEGREES_TO_RADIANS ;
1287 hpr[1] = p * SG_DEGREES_TO_RADIANS ;
1288 hpr[2] = r * SG_DEGREES_TO_RADIANS ;
1289
1290 sgEulerToQuat( dst, hpr ) ;
1291 }
1292
sgHPRToQuat(sgQuat dst,const sgVec3 hpr)1293 inline void sgHPRToQuat ( sgQuat dst, const sgVec3 hpr )
1294 {
1295 sgVec3 tmp ;
1296
1297 sgScaleVec3 ( tmp, hpr, SG_DEGREES_TO_RADIANS ) ;
1298
1299 sgEulerToQuat ( dst, tmp ) ;
1300 }
1301
1302
1303 /* Multiply quaternions together (concatenate rotations) */
1304
1305 void sgMultQuat ( sgQuat dst, const sgQuat a, const sgQuat b ) ;
1306
sgPostMultQuat(sgQuat dst,const sgQuat q)1307 inline void sgPostMultQuat ( sgQuat dst, const sgQuat q )
1308 {
1309 sgQuat r ;
1310
1311 sgCopyQuat ( r, dst ) ;
1312 sgMultQuat ( dst, r, q ) ;
1313 }
1314
sgPreMultQuat(sgQuat dst,const sgQuat q)1315 inline void sgPreMultQuat ( sgQuat dst, const sgQuat q )
1316 {
1317 sgQuat r ;
1318
1319 sgCopyQuat ( r, dst ) ;
1320 sgMultQuat ( dst, q, r ) ;
1321 }
1322
1323
1324 /* Rotate a quaternion by a given angle and axis (convenience function) */
1325
1326
sgPreRotQuat(sgQuat dst,const SGfloat angle,const sgVec3 axis)1327 inline void sgPreRotQuat ( sgQuat dst, const SGfloat angle, const sgVec3 axis )
1328 {
1329 sgQuat q ;
1330 sgAngleAxisToQuat ( q, angle, axis ) ;
1331 sgPreMultQuat ( dst, q ) ;
1332 sgNormaliseQuat ( dst ) ;
1333 }
1334
1335
sgPostRotQuat(sgQuat dst,const SGfloat angle,const sgVec3 axis)1336 inline void sgPostRotQuat ( sgQuat dst, const SGfloat angle, const sgVec3 axis )
1337 {
1338 sgQuat q ;
1339 sgAngleAxisToQuat ( q, angle, axis ) ;
1340 sgPostMultQuat ( dst, q ) ;
1341 sgNormaliseQuat ( dst ) ;
1342 }
1343
1344
sgPreRotQuat(sgQuat dst,const SGfloat angle,const SGfloat x,const SGfloat y,const SGfloat z)1345 inline void sgPreRotQuat ( sgQuat dst,
1346 const SGfloat angle,
1347 const SGfloat x, const SGfloat y, const SGfloat z )
1348 {
1349 sgVec3 axis ;
1350
1351 sgSetVec3 ( axis, x, y, z ) ;
1352 sgPreRotQuat ( dst, angle, axis ) ;
1353 }
1354
sgPostRotQuat(sgQuat dst,const SGfloat angle,const SGfloat x,const SGfloat y,const SGfloat z)1355 inline void sgPostRotQuat ( sgQuat dst,
1356 const SGfloat angle,
1357 const SGfloat x, const SGfloat y, const SGfloat z )
1358 {
1359 sgVec3 axis ;
1360
1361 sgSetVec3 ( axis, x, y, z ) ;
1362 sgPostRotQuat ( dst, angle, axis ) ;
1363 }
1364
1365
1366 /* more meaningful names */
1367 #define sgWorldRotQuat sgPostRotQuat
1368 #define sgLocalRotQuat sgPreRotQuat
1369
1370 /* for backwards compatibility */
1371 #define sgRotQuat sgPostRotQuat
1372
1373
1374 /* SWC - Interpolate between to quaternions */
1375
1376 extern void sgSlerpQuat ( sgQuat dst,
1377 const sgQuat from, const sgQuat to,
1378 const SGfloat t ) ;
1379
1380
1381
1382 /*
1383 Intersection testing.
1384 */
1385
1386 int sgIsectPlanePlane ( sgVec3 point, sgVec3 dir,
1387 sgVec4 plane1, sgVec4 plane2 ) ;
1388 int sgIsectInfLinePlane ( sgVec3 dst,
1389 sgVec3 l_org, sgVec3 l_vec,
1390 sgVec4 plane ) ;
1391 int sgIsectInfLineInfLine ( sgVec3 dst,
1392 sgVec3 l1_org, sgVec3 l1_vec,
1393 sgVec3 l2_org, sgVec3 l2_vec ) ;
1394 SGfloat sgIsectLinesegPlane ( sgVec3 dst,
1395 sgVec3 v1, sgVec3 v2,
1396 sgVec4 plane ) ;
1397
1398 bool sgPointInTriangle3 ( sgVec3 point, sgVec3 tri[3] ) ;
1399 bool sgPointInTriangle2 ( sgVec2 point, sgVec2 tri[3] ) ;
1400
sgPointInTriangle(sgVec3 point,sgVec3 tri[3])1401 inline bool sgPointInTriangle ( sgVec3 point, sgVec3 tri[3] )
1402 {
1403 return sgPointInTriangle3 ( point, tri ) ;
1404 }
1405
1406
1407
1408 /**********************************************************************/
1409
1410 #define sgdFloat double
1411 #define SGDfloat double
1412
1413 #define SGD_ZERO 0.0
1414 #define SGD_HALF 0.5
1415 #define SGD_ONE 1.0
1416 #define SGD_TWO 2.0
1417 #define SGD_THREE 3.0
1418 #define SGD_FOUR 4.0
1419 #define SGD_45 45.0
1420 #define SGD_60 60.0
1421 #define SGD_90 90.0
1422 #define SGD_180 180.0
1423 #define SGD_MAX DBL_MAX
1424
1425
1426 #define SGD_X 0
1427 #define SGD_Y 1
1428 #define SGD_Z 2
1429 #define SGD_W 3
1430
1431 #ifndef M_PI
1432 #define SGD_PI 3.14159265358979323846 /* From M_PI under Linux/X86 */
1433 #else
1434 #define SGD_PI M_PI
1435 #endif
1436
1437 #define SGD_DEGREES_TO_RADIANS (SGD_PI/SGD_180)
1438 #define SGD_RADIANS_TO_DEGREES (SGD_180/SGD_PI)
1439
1440 /*
1441 These are just convenient redefinitions of standard
1442 math library functions to stop float/double warnings.
1443 */
1444
sgdSqrt(const SGDfloat x)1445 inline SGDfloat sgdSqrt ( const SGDfloat x ) { return sqrt ( x ) ; }
sgdSquare(const SGDfloat x)1446 inline SGDfloat sgdSquare ( const SGDfloat x ) { return x * x ; }
sgdAbs(const SGDfloat a)1447 inline SGDfloat sgdAbs ( const SGDfloat a ) { return ( a < SGD_ZERO ) ? -a : a ; }
1448
sgdASin(SGDfloat s)1449 inline SGDfloat sgdASin ( SGDfloat s )
1450 { return (SGDfloat) asin ( s ) * SGD_RADIANS_TO_DEGREES ; }
sgdACos(SGDfloat s)1451 inline SGDfloat sgdACos ( SGDfloat s )
1452 { return (SGDfloat) acos ( s ) * SGD_RADIANS_TO_DEGREES ; }
sgdATan(SGDfloat s)1453 inline SGDfloat sgdATan ( SGDfloat s )
1454 { return (SGDfloat) atan ( s ) * SGD_RADIANS_TO_DEGREES ; }
sgdATan2(SGDfloat y,SGDfloat x)1455 inline SGDfloat sgdATan2 ( SGDfloat y, SGDfloat x )
1456 { return (SGDfloat) atan2 ( y,x ) * SGD_RADIANS_TO_DEGREES ; }
sgdSin(SGDfloat s)1457 inline SGDfloat sgdSin ( SGDfloat s )
1458 { return (SGDfloat) sin ( s * SGD_DEGREES_TO_RADIANS ) ; }
sgdCos(SGDfloat s)1459 inline SGDfloat sgdCos ( SGDfloat s )
1460 { return (SGDfloat) cos ( s * SGD_DEGREES_TO_RADIANS ) ; }
sgdTan(SGDfloat s)1461 inline SGDfloat sgdTan ( SGDfloat s )
1462 { return (SGDfloat) tan ( s * SGD_DEGREES_TO_RADIANS ) ; }
1463
sgdCompareFloat(const SGDfloat a,const SGDfloat b,const SGDfloat tol)1464 inline int sgdCompareFloat ( const SGDfloat a, const SGDfloat b, const SGDfloat tol )
1465 {
1466 if ( ( a + tol ) < b ) return -1 ;
1467 if ( ( b + tol ) < a ) return 1 ;
1468 return 0 ;
1469 }
1470
1471
1472 /*
1473 Types used in SGD.
1474 */
1475
1476 typedef SGDfloat sgdVec2 [ 2 ] ;
1477 typedef SGDfloat sgdVec3 [ 3 ] ;
1478 typedef SGDfloat sgdVec4 [ 4 ] ;
1479
1480 typedef sgdVec4 sgdQuat ;
1481
1482 typedef SGDfloat sgdMat3 [3][3] ;
1483 typedef SGDfloat sgdMat4 [4][4] ;
1484
1485 struct sgdCoord
1486 {
1487 sgdVec3 xyz ;
1488 sgdVec3 hpr ;
1489 } ;
1490
1491 class sgdSphere ;
1492 class sgdBox ;
1493 class sgdFrustum ;
1494
1495 /*
1496 Some handy constants
1497 */
1498
1499 #define SGD_OUTSIDE FALSE
1500 #define SGD_INSIDE TRUE
1501 #define SGD_STRADDLE 2
1502
sgdHeightOfPlaneVec2(const sgdVec4 plane,const sgdVec2 pnt)1503 inline SGDfloat sgdHeightOfPlaneVec2 ( const sgdVec4 plane, const sgdVec2 pnt )
1504 {
1505 if ( plane[2] == SGD_ZERO )
1506 return SGD_ZERO ;
1507 else
1508 return -( plane[0] * pnt[0] + plane[1] * pnt[1] + plane[3] ) / plane[2] ;
1509 }
1510
1511 /*
1512 Convert a direction vector into a set of euler angles,
1513 (with zero roll)
1514 */
1515
1516 extern void sgdHPRfromVec3 ( sgdVec3 hpr, const sgdVec3 src ) ;
1517
1518 extern void sgdMakeCoordMat4 ( sgdMat4 dst, const SGDfloat x, const SGDfloat y, const SGDfloat z,
1519 const SGDfloat h, const SGDfloat p, const SGDfloat r ) ;
1520
sgdMakeCoordMat4(sgdMat4 dst,const sgdVec3 xyz,const sgdVec3 hpr)1521 inline void sgdMakeCoordMat4( sgdMat4 dst, const sgdVec3 xyz, const sgdVec3 hpr )
1522 {
1523 sgdMakeCoordMat4 ( dst, xyz[0], xyz[1], xyz[2],
1524 hpr[0], hpr[1], hpr[2] ) ;
1525 }
1526
sgdMakeCoordMat4(sgdMat4 dst,const sgdCoord * src)1527 inline void sgdMakeCoordMat4( sgdMat4 dst, const sgdCoord *src )
1528 {
1529 sgdMakeCoordMat4 ( dst, src->xyz, src->hpr ) ;
1530 }
1531
1532 extern void sgdMakeRotMat4 ( sgdMat4 dst, const SGDfloat angle, const sgdVec3 axis ) ;
1533
sgdMakeRotMat4(sgdMat4 dst,const sgdVec3 hpr)1534 inline void sgdMakeRotMat4 ( sgdMat4 dst, const sgdVec3 hpr )
1535 {
1536 sgdMakeCoordMat4 ( dst, SGD_ZERO, SGD_ZERO, SGD_ZERO, hpr[0], hpr[1], hpr[2] ) ;
1537 }
1538
sgdMakeRotMat4(sgdMat4 dst,const SGDfloat h,const SGDfloat p,const SGDfloat r)1539 inline void sgdMakeRotMat4 ( sgdMat4 dst, const SGDfloat h, const SGDfloat p, const SGDfloat r )
1540 {
1541 sgdMakeCoordMat4 ( dst, SGD_ZERO, SGD_ZERO, SGD_ZERO, h, p, r ) ;
1542 }
1543
1544 extern void sgdMakeTransMat4 ( sgdMat4 dst, const sgdVec3 xyz ) ;
1545 extern void sgdMakeTransMat4 ( sgdMat4 dst, const SGDfloat x, const SGDfloat y, const SGDfloat z ) ;
1546
1547
1548 extern void sgdSetCoord ( sgdCoord *coord, const sgdMat4 src ) ;
1549
1550 extern void sgdMultMat4 ( sgdMat4 dst, const sgdMat4 a, const sgdMat4 b ) ;
1551 extern void sgdPostMultMat4 ( sgdMat4 dst, const sgdMat4 a ) ;
1552 extern void sgdPreMultMat4 ( sgdMat4 dst, const sgdMat4 a ) ;
1553
1554 extern void sgdTransposeNegateMat4 ( sgdMat4 dst ) ;
1555 extern void sgdTransposeNegateMat4 ( sgdMat4 dst, const sgdMat4 src ) ;
1556
1557 extern void sgdInvertMat4 ( sgdMat4 dst, const sgdMat4 src ) ;
sgdInvertMat4(sgdMat4 dst)1558 inline void sgdInvertMat4 ( sgdMat4 dst ) { sgdInvertMat4 ( dst, dst ) ; }
1559
1560 extern void sgdXformVec3 ( sgdVec3 dst, const sgdVec3 src, const sgdMat4 mat ) ;
1561 extern void sgdXformPnt3 ( sgdVec3 dst, const sgdVec3 src, const sgdMat4 mat ) ;
1562 extern void sgdXformVec4 ( sgdVec4 dst, const sgdVec4 src, const sgdMat4 mat ) ;
1563 extern void sgdXformPnt4 ( sgdVec4 dst, const sgdVec4 src, const sgdMat4 mat ) ;
1564 extern void sgdFullXformPnt3 ( sgdVec3 dst, const sgdVec3 src, const sgdMat4 mat ) ;
1565 extern void sgdFullXformPnt4 ( sgdVec4 dst, const sgdVec4 src, const sgdMat4 mat ) ;
1566
sgdXformVec3(sgdVec3 dst,const sgdMat4 mat)1567 inline void sgdXformVec3 ( sgdVec3 dst, const sgdMat4 mat ) { sgdXformVec3 ( dst, dst, mat ) ; }
sgdXformPnt3(sgdVec3 dst,const sgdMat4 mat)1568 inline void sgdXformPnt3 ( sgdVec3 dst, const sgdMat4 mat ) { sgdXformPnt3 ( dst, dst, mat ) ; }
sgdXformVec4(sgdVec4 dst,const sgdMat4 mat)1569 inline void sgdXformVec4 ( sgdVec4 dst, const sgdMat4 mat ) { sgdXformVec4 ( dst, dst, mat ) ; }
sgdXformPnt4(sgdVec4 dst,const sgdMat4 mat)1570 inline void sgdXformPnt4 ( sgdVec4 dst, const sgdMat4 mat ) { sgdXformPnt4 ( dst, dst, mat ) ; }
sgdFullXformPnt3(sgdVec3 dst,const sgdMat4 mat)1571 inline void sgdFullXformPnt3 ( sgdVec3 dst, const sgdMat4 mat ) { sgdFullXformPnt3 ( dst, dst, mat ) ; }
sgdFullXformPnt4(sgdVec4 dst,const sgdMat4 mat)1572 inline void sgdFullXformPnt4 ( sgdVec4 dst, const sgdMat4 mat ) { sgdFullXformPnt4 ( dst, dst, mat ) ; }
1573
1574 /*
1575 Basic low-level vector functions.
1576 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1577 For each of Vec2, Vec3 and Vec4, we provide inlines for
1578
1579 Zero - set all elements to zero.
1580 Set - set each element individually.
1581 Add - add vectors element by element.
1582 Sub - subtract vectors element by element.
1583 Scale - multiply each element of a vector by a variable.
1584 Negate - negate each element of a vector.
1585 Compare - compare vectors element by element with optional tolerance.
1586 (return TRUE if vectors are equal - within tolerances)
1587 Equal - return TRUE if vectors are exactly equal.
1588 Length - compute length of a vector.
1589 Distance - compute distance between two points.
1590 LengthSquared - compute the square of the length of a vector.
1591 DistanceSquared - compute the square of the distance between two points.
1592 ScalarProduct - scalar (dot) product.
1593 VectorProduct - vector (cross) product (3-element vectors ONLY!).
1594 Normalise/Normalize - make vector be one unit long.
1595 Lerp - linear interpolation by a fraction 'f'.
1596 */
1597
sgdZeroVec2(sgdVec2 dst)1598 inline void sgdZeroVec2 ( sgdVec2 dst ) { dst[0]=dst[1]=SGD_ZERO ; }
sgdZeroVec3(sgdVec3 dst)1599 inline void sgdZeroVec3 ( sgdVec3 dst ) { dst[0]=dst[1]=dst[2]=SGD_ZERO ; }
sgdZeroVec4(sgdVec4 dst)1600 inline void sgdZeroVec4 ( sgdVec4 dst ) { dst[0]=dst[1]=dst[2]=dst[3]=SGD_ZERO ; }
1601
1602
sgdSetVec2(sgdVec2 dst,const SGDfloat x,const SGDfloat y)1603 inline void sgdSetVec2 ( sgdVec2 dst, const SGDfloat x, const SGDfloat y )
1604 {
1605 dst [ 0 ] = x ;
1606 dst [ 1 ] = y ;
1607 }
1608
sgdSetVec3(sgdVec3 dst,const SGDfloat x,const SGDfloat y,const SGDfloat z)1609 inline void sgdSetVec3 ( sgdVec3 dst, const SGDfloat x, const SGDfloat y, const SGDfloat z )
1610 {
1611 dst [ 0 ] = x ;
1612 dst [ 1 ] = y ;
1613 dst [ 2 ] = z ;
1614 }
1615
sgdSetVec4(sgdVec4 dst,const SGDfloat x,const SGDfloat y,const SGDfloat z,const SGDfloat w)1616 inline void sgdSetVec4 ( sgdVec4 dst, const SGDfloat x, const SGDfloat y, const SGDfloat z, const SGDfloat w )
1617 {
1618 dst [ 0 ] = x ;
1619 dst [ 1 ] = y ;
1620 dst [ 2 ] = z ;
1621 dst [ 3 ] = w ;
1622 }
1623
1624
sgdCopyVec2(sgdVec2 dst,const sgdVec2 src)1625 inline void sgdCopyVec2 ( sgdVec2 dst, const sgdVec2 src )
1626 {
1627 dst [ 0 ] = src [ 0 ] ;
1628 dst [ 1 ] = src [ 1 ] ;
1629 }
1630
sgdCopyVec3(sgdVec3 dst,const sgdVec3 src)1631 inline void sgdCopyVec3 ( sgdVec3 dst, const sgdVec3 src )
1632 {
1633 dst [ 0 ] = src [ 0 ] ;
1634 dst [ 1 ] = src [ 1 ] ;
1635 dst [ 2 ] = src [ 2 ] ;
1636 }
1637
sgdCopyVec4(sgdVec4 dst,const sgdVec4 src)1638 inline void sgdCopyVec4 ( sgdVec4 dst, const sgdVec4 src )
1639 {
1640 dst [ 0 ] = src [ 0 ] ;
1641 dst [ 1 ] = src [ 1 ] ;
1642 dst [ 2 ] = src [ 2 ] ;
1643 dst [ 3 ] = src [ 3 ] ;
1644 }
1645
1646
sgdAddVec2(sgdVec2 dst,const sgdVec2 src)1647 inline void sgdAddVec2 ( sgdVec2 dst, const sgdVec2 src )
1648 {
1649 dst [ 0 ] += src [ 0 ] ;
1650 dst [ 1 ] += src [ 1 ] ;
1651 }
1652
sgdAddVec3(sgdVec3 dst,const sgdVec3 src)1653 inline void sgdAddVec3 ( sgdVec3 dst, const sgdVec3 src )
1654 {
1655 dst [ 0 ] += src [ 0 ] ;
1656 dst [ 1 ] += src [ 1 ] ;
1657 dst [ 2 ] += src [ 2 ] ;
1658 }
1659
sgdAddVec4(sgdVec4 dst,const sgdVec4 src)1660 inline void sgdAddVec4 ( sgdVec4 dst, const sgdVec4 src )
1661 {
1662 dst [ 0 ] += src [ 0 ] ;
1663 dst [ 1 ] += src [ 1 ] ;
1664 dst [ 2 ] += src [ 2 ] ;
1665 dst [ 3 ] += src [ 3 ] ;
1666 }
1667
1668
sgdAddVec2(sgdVec2 dst,const sgdVec2 src1,const sgdVec2 src2)1669 inline void sgdAddVec2 ( sgdVec2 dst, const sgdVec2 src1, const sgdVec2 src2 )
1670 {
1671 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] ;
1672 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] ;
1673 }
1674
sgdAddVec3(sgdVec3 dst,const sgdVec3 src1,const sgdVec3 src2)1675 inline void sgdAddVec3 ( sgdVec3 dst, const sgdVec3 src1, const sgdVec3 src2 )
1676 {
1677 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] ;
1678 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] ;
1679 dst [ 2 ] = src1 [ 2 ] + src2 [ 2 ] ;
1680 }
1681
sgdAddVec4(sgdVec4 dst,const sgdVec4 src1,const sgdVec4 src2)1682 inline void sgdAddVec4 ( sgdVec4 dst, const sgdVec4 src1, const sgdVec4 src2 )
1683 {
1684 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] ;
1685 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] ;
1686 dst [ 2 ] = src1 [ 2 ] + src2 [ 2 ] ;
1687 dst [ 3 ] = src1 [ 3 ] + src2 [ 3 ] ;
1688 }
1689
1690
sgdSubVec2(sgdVec2 dst,const sgdVec2 src)1691 inline void sgdSubVec2 ( sgdVec2 dst, const sgdVec2 src )
1692 {
1693 dst [ 0 ] -= src [ 0 ] ;
1694 dst [ 1 ] -= src [ 1 ] ;
1695 }
1696
sgdSubVec3(sgdVec3 dst,const sgdVec3 src)1697 inline void sgdSubVec3 ( sgdVec3 dst, const sgdVec3 src )
1698 {
1699 dst [ 0 ] -= src [ 0 ] ;
1700 dst [ 1 ] -= src [ 1 ] ;
1701 dst [ 2 ] -= src [ 2 ] ;
1702 }
1703
sgdSubVec4(sgdVec4 dst,const sgdVec4 src)1704 inline void sgdSubVec4 ( sgdVec4 dst, const sgdVec4 src )
1705 {
1706 dst [ 0 ] -= src [ 0 ] ;
1707 dst [ 1 ] -= src [ 1 ] ;
1708 dst [ 2 ] -= src [ 2 ] ;
1709 dst [ 3 ] -= src [ 3 ] ;
1710 }
1711
sgdSubVec2(sgdVec2 dst,const sgdVec2 src1,const sgdVec2 src2)1712 inline void sgdSubVec2 ( sgdVec2 dst, const sgdVec2 src1, const sgdVec2 src2 )
1713 {
1714 dst [ 0 ] = src1 [ 0 ] - src2 [ 0 ] ;
1715 dst [ 1 ] = src1 [ 1 ] - src2 [ 1 ] ;
1716 }
1717
sgdSubVec3(sgdVec3 dst,const sgdVec3 src1,const sgdVec3 src2)1718 inline void sgdSubVec3 ( sgdVec3 dst, const sgdVec3 src1, const sgdVec3 src2 )
1719 {
1720 dst [ 0 ] = src1 [ 0 ] - src2 [ 0 ] ;
1721 dst [ 1 ] = src1 [ 1 ] - src2 [ 1 ] ;
1722 dst [ 2 ] = src1 [ 2 ] - src2 [ 2 ] ;
1723 }
1724
sgdSubVec4(sgdVec4 dst,const sgdVec4 src1,const sgdVec4 src2)1725 inline void sgdSubVec4 ( sgdVec4 dst, const sgdVec4 src1, const sgdVec4 src2 )
1726 {
1727 dst [ 0 ] = src1 [ 0 ] - src2 [ 0 ] ;
1728 dst [ 1 ] = src1 [ 1 ] - src2 [ 1 ] ;
1729 dst [ 2 ] = src1 [ 2 ] - src2 [ 2 ] ;
1730 dst [ 3 ] = src1 [ 3 ] - src2 [ 3 ] ;
1731 }
1732
1733
sgdNegateVec2(sgdVec2 dst)1734 inline void sgdNegateVec2 ( sgdVec2 dst )
1735 {
1736 dst [ 0 ] = -dst [ 0 ] ;
1737 dst [ 1 ] = -dst [ 1 ] ;
1738 }
1739
sgdNegateVec3(sgdVec3 dst)1740 inline void sgdNegateVec3 ( sgdVec3 dst )
1741 {
1742 dst [ 0 ] = -dst [ 0 ] ;
1743 dst [ 1 ] = -dst [ 1 ] ;
1744 dst [ 2 ] = -dst [ 2 ] ;
1745 }
1746
sgdNegateVec4(sgdVec4 dst)1747 inline void sgdNegateVec4 ( sgdVec4 dst )
1748 {
1749 dst [ 0 ] = -dst [ 0 ] ;
1750 dst [ 1 ] = -dst [ 1 ] ;
1751 dst [ 2 ] = -dst [ 2 ] ;
1752 dst [ 3 ] = -dst [ 3 ] ;
1753 }
1754
1755
sgdNegateVec2(sgdVec2 dst,const sgdVec2 src)1756 inline void sgdNegateVec2 ( sgdVec2 dst, const sgdVec2 src )
1757 {
1758 dst [ 0 ] = -src [ 0 ] ;
1759 dst [ 1 ] = -src [ 1 ] ;
1760 }
1761
sgdNegateVec3(sgdVec3 dst,const sgdVec3 src)1762 inline void sgdNegateVec3 ( sgdVec3 dst, const sgdVec3 src )
1763 {
1764 dst [ 0 ] = -src [ 0 ] ;
1765 dst [ 1 ] = -src [ 1 ] ;
1766 dst [ 2 ] = -src [ 2 ] ;
1767 }
1768
sgdNegateVec4(sgdVec4 dst,const sgdVec4 src)1769 inline void sgdNegateVec4 ( sgdVec4 dst, const sgdVec4 src )
1770 {
1771 dst [ 0 ] = -src [ 0 ] ;
1772 dst [ 1 ] = -src [ 1 ] ;
1773 dst [ 2 ] = -src [ 2 ] ;
1774 dst [ 3 ] = -src [ 3 ] ;
1775 }
1776
1777
sgdScaleVec2(sgdVec2 dst,const SGDfloat s)1778 inline void sgdScaleVec2 ( sgdVec2 dst, const SGDfloat s )
1779 {
1780 dst [ 0 ] *= s ;
1781 dst [ 1 ] *= s ;
1782 }
1783
sgdScaleVec3(sgdVec3 dst,const SGDfloat s)1784 inline void sgdScaleVec3 ( sgdVec3 dst, const SGDfloat s )
1785 {
1786 dst [ 0 ] *= s ;
1787 dst [ 1 ] *= s ;
1788 dst [ 2 ] *= s ;
1789 }
1790
sgdScaleVec4(sgdVec4 dst,const SGDfloat s)1791 inline void sgdScaleVec4 ( sgdVec4 dst, const SGDfloat s )
1792 {
1793 dst [ 0 ] *= s ;
1794 dst [ 1 ] *= s ;
1795 dst [ 2 ] *= s ;
1796 dst [ 3 ] *= s ;
1797 }
1798
sgdScaleVec2(sgdVec2 dst,const sgdVec2 src,const SGDfloat s)1799 inline void sgdScaleVec2 ( sgdVec2 dst, const sgdVec2 src, const SGDfloat s )
1800 {
1801 dst [ 0 ] = src [ 0 ] * s ;
1802 dst [ 1 ] = src [ 1 ] * s ;
1803 }
1804
sgdScaleVec3(sgdVec3 dst,const sgdVec3 src,const SGDfloat s)1805 inline void sgdScaleVec3 ( sgdVec3 dst, const sgdVec3 src, const SGDfloat s )
1806 {
1807 dst [ 0 ] = src [ 0 ] * s ;
1808 dst [ 1 ] = src [ 1 ] * s ;
1809 dst [ 2 ] = src [ 2 ] * s ;
1810 }
1811
sgdScaleVec4(sgdVec4 dst,const sgdVec4 src,const SGDfloat s)1812 inline void sgdScaleVec4 ( sgdVec4 dst, const sgdVec4 src, const SGDfloat s )
1813 {
1814 dst [ 0 ] = src [ 0 ] * s ;
1815 dst [ 1 ] = src [ 1 ] * s ;
1816 dst [ 2 ] = src [ 2 ] * s ;
1817 dst [ 3 ] = src [ 3 ] * s ;
1818 }
1819
1820
sgdAddScaledVec2(sgdVec2 dst,const sgdVec2 src,const SGDfloat s)1821 inline void sgdAddScaledVec2 ( sgdVec2 dst, const sgdVec2 src, const SGDfloat s )
1822 {
1823 dst [ 0 ] += src [ 0 ] * s ;
1824 dst [ 1 ] += src [ 1 ] * s ;
1825 }
1826
sgdAddScaledVec3(sgdVec3 dst,const sgdVec3 src,const SGDfloat s)1827 inline void sgdAddScaledVec3 ( sgdVec3 dst, const sgdVec3 src, const SGDfloat s )
1828 {
1829 dst [ 0 ] += src [ 0 ] * s ;
1830 dst [ 1 ] += src [ 1 ] * s ;
1831 dst [ 2 ] += src [ 2 ] * s ;
1832 }
1833
sgdAddScaledVec4(sgdVec4 dst,const sgdVec4 src,const SGDfloat s)1834 inline void sgdAddScaledVec4 ( sgdVec4 dst, const sgdVec4 src, const SGDfloat s )
1835 {
1836 dst [ 0 ] += src [ 0 ] * s ;
1837 dst [ 1 ] += src [ 1 ] * s ;
1838 dst [ 2 ] += src [ 2 ] * s ;
1839 dst [ 3 ] += src [ 3 ] * s ;
1840 }
1841
1842
sgdAddScaledVec2(sgdVec2 dst,const sgdVec2 src1,const sgdVec2 src2,const SGDfloat s)1843 inline void sgdAddScaledVec2 ( sgdVec2 dst, const sgdVec2 src1, const sgdVec2 src2, const SGDfloat s )
1844 {
1845 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] * s ;
1846 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] * s ;
1847 }
1848
sgdAddScaledVec3(sgdVec3 dst,const sgdVec3 src1,const sgdVec3 src2,const SGDfloat s)1849 inline void sgdAddScaledVec3 ( sgdVec3 dst, const sgdVec3 src1, const sgdVec3 src2, const SGDfloat s )
1850 {
1851 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] * s ;
1852 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] * s ;
1853 dst [ 2 ] = src1 [ 2 ] + src2 [ 2 ] * s ;
1854 }
1855
sgdAddScaledVec4(sgdVec4 dst,const sgdVec4 src1,const sgdVec4 src2,const SGDfloat s)1856 inline void sgdAddScaledVec4 ( sgdVec4 dst, const sgdVec4 src1, const sgdVec4 src2, const SGDfloat s )
1857 {
1858 dst [ 0 ] = src1 [ 0 ] + src2 [ 0 ] * s ;
1859 dst [ 1 ] = src1 [ 1 ] + src2 [ 1 ] * s ;
1860 dst [ 2 ] = src1 [ 2 ] + src2 [ 2 ] * s ;
1861 dst [ 3 ] = src1 [ 3 ] + src2 [ 3 ] * s ;
1862 }
1863
1864
1865
sgdCompareVec2(const sgdVec2 a,const sgdVec2 b,const SGDfloat tol)1866 inline int sgdCompareVec2 ( const sgdVec2 a, const sgdVec2 b, const SGDfloat tol )
1867 {
1868 int val = 0 ;
1869
1870 if ( ( val = sgdCompareFloat( a[0], b[0], tol ) ) != 0 ) return val ;
1871 if ( ( val = sgdCompareFloat( a[1], b[1], tol ) ) != 0 ) return val ;
1872
1873 return 0 ;
1874 }
1875
sgdCompareVec3(const sgdVec3 a,const sgdVec3 b,const SGDfloat tol)1876 inline int sgdCompareVec3 ( const sgdVec3 a, const sgdVec3 b, const SGDfloat tol )
1877 {
1878 int val = 0 ;
1879
1880 if ( ( val = sgdCompareFloat( a[0], b[0], tol ) ) != 0 ) return val ;
1881 if ( ( val = sgdCompareFloat( a[1], b[1], tol ) ) != 0 ) return val ;
1882 if ( ( val = sgdCompareFloat( a[2], b[2], tol ) ) != 0 ) return val ;
1883
1884 return 0 ;
1885 }
1886
sgdCompareVec4(const sgdVec4 a,const sgdVec4 b,const SGDfloat tol)1887 inline int sgdCompareVec4 ( const sgdVec4 a, const sgdVec4 b, const SGDfloat tol )
1888 {
1889 int val = 0 ;
1890
1891 if ( ( val = sgdCompareFloat( a[0], b[0], tol ) ) != 0 ) return val ;
1892 if ( ( val = sgdCompareFloat( a[1], b[1], tol ) ) != 0 ) return val ;
1893 if ( ( val = sgdCompareFloat( a[2], b[2], tol ) ) != 0 ) return val ;
1894 if ( ( val = sgdCompareFloat( a[3], b[3], tol ) ) != 0 ) return val ;
1895
1896 return 0 ;
1897 }
1898
1899
sgdEqualVec2(const sgdVec2 a,const sgdVec2 b)1900 inline int sgdEqualVec2 ( const sgdVec2 a, const sgdVec2 b )
1901 {
1902 return a[0] == b[0] &&
1903 a[1] == b[1] ;
1904 }
1905
sgdEqualVec3(const sgdVec3 a,const sgdVec3 b)1906 inline int sgdEqualVec3 ( const sgdVec3 a, const sgdVec3 b )
1907 {
1908 return a[0] == b[0] &&
1909 a[1] == b[1] &&
1910 a[2] == b[2] ;
1911 }
1912
sgdEqualVec4(const sgdVec4 a,const sgdVec4 b)1913 inline int sgdEqualVec4 ( const sgdVec4 a, const sgdVec4 b )
1914 {
1915 return a[0] == b[0] &&
1916 a[1] == b[1] &&
1917 a[2] == b[2] &&
1918 a[3] == b[3] ;
1919 }
1920
1921
sgdScalarProductVec2(const sgdVec2 a,const sgdVec2 b)1922 inline SGDfloat sgdScalarProductVec2 ( const sgdVec2 a, const sgdVec2 b )
1923 {
1924 return a[0]*b[0] + a[1]*b[1] ;
1925 }
1926
sgdScalarProductVec3(const sgdVec3 a,const sgdVec3 b)1927 inline SGDfloat sgdScalarProductVec3 ( const sgdVec3 a, const sgdVec3 b )
1928 {
1929 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] ;
1930 }
1931
sgdScalarProductVec4(const sgdVec4 a,const sgdVec4 b)1932 inline SGDfloat sgdScalarProductVec4 ( const sgdVec4 a, const sgdVec4 b )
1933 {
1934 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3] ;
1935 }
1936
1937
1938 extern void sgdVectorProductVec3 ( sgdVec3 dst, const sgdVec3 a, const sgdVec3 b ) ;
1939
1940
sgdLerpVec4(sgdVec4 dst,const sgdVec4 a,const sgdVec4 b,const SGDfloat f)1941 inline void sgdLerpVec4 ( sgdVec4 dst, const sgdVec4 a, const sgdVec4 b, const SGDfloat f )
1942 {
1943 dst[0] = a[0] + f * ( b[0] - a[0] ) ;
1944 dst[1] = a[1] + f * ( b[1] - a[1] ) ;
1945 dst[2] = a[2] + f * ( b[2] - a[2] ) ;
1946 dst[3] = a[3] + f * ( b[3] - a[3] ) ;
1947 }
1948
sgdLerpVec3(sgdVec3 dst,const sgdVec3 a,const sgdVec3 b,const SGDfloat f)1949 inline void sgdLerpVec3 ( sgdVec3 dst, const sgdVec3 a, const sgdVec3 b, const SGDfloat f )
1950 {
1951 dst[0] = a[0] + f * ( b[0] - a[0] ) ;
1952 dst[1] = a[1] + f * ( b[1] - a[1] ) ;
1953 dst[2] = a[2] + f * ( b[2] - a[2] ) ;
1954 }
1955
sgdLerpVec2(sgdVec2 dst,const sgdVec2 a,const sgdVec2 b,const SGDfloat f)1956 inline void sgdLerpVec2 ( sgdVec2 dst, const sgdVec2 a, const sgdVec2 b, const SGDfloat f )
1957 {
1958 dst[0] = a[0] + f * ( b[0] - a[0] ) ;
1959 dst[1] = a[1] + f * ( b[1] - a[1] ) ;
1960 }
1961
sgdLerpAnglesVec3(sgdVec3 dst,const sgdVec3 a,const sgdVec3 b,const SGDfloat f)1962 inline void sgdLerpAnglesVec3 ( sgdVec3 dst, const sgdVec3 a,
1963 const sgdVec3 b,
1964 const SGDfloat f )
1965 {
1966 sgdVec3 tmp ;
1967
1968 if ( b[0] - a[0] > 180.0 ) tmp[0] = a[0] + 360.0 ; else
1969 if ( b[0] - a[0] < -180.0 ) tmp[0] = a[0] - 360.0 ; else tmp[0] = a[0] ;
1970
1971 if ( b[1] - a[1] > 180.0 ) tmp[1] = a[1] + 360.0 ; else
1972 if ( b[1] - a[1] < -180.0 ) tmp[1] = a[1] - 360.0 ; else tmp[1] = a[1] ;
1973
1974 if ( b[2] - a[2] > 180.0 ) tmp[2] = a[2] + 360.0 ; else
1975 if ( b[2] - a[2] < -180.0 ) tmp[2] = a[2] - 360.0 ; else tmp[2] = a[2] ;
1976
1977 dst[0] = tmp[0] + f * ( b[0] - tmp[0] ) ;
1978 dst[1] = tmp[1] + f * ( b[1] - tmp[1] ) ;
1979 dst[2] = tmp[2] + f * ( b[2] - tmp[2] ) ;
1980 }
1981
1982
1983
1984
sgdDistanceSquaredVec2(const sgdVec2 a,const sgdVec2 b)1985 inline SGDfloat sgdDistanceSquaredVec2 ( const sgdVec2 a, const sgdVec2 b )
1986 {
1987 return sgdSquare ( a[0]-b[0] ) + sgdSquare ( a[1]-b[1] ) ;
1988 }
1989
sgdDistanceSquaredVec3(const sgdVec3 a,const sgdVec3 b)1990 inline SGDfloat sgdDistanceSquaredVec3 ( const sgdVec3 a, const sgdVec3 b )
1991 {
1992 return sgdSquare ( a[0]-b[0] ) + sgdSquare ( a[1]-b[1] ) +
1993 sgdSquare ( a[2]-b[2] ) ;
1994 }
1995
sgdDistanceSquaredVec4(const sgdVec4 a,const sgdVec4 b)1996 inline SGDfloat sgdDistanceSquaredVec4 ( const sgdVec4 a, const sgdVec4 b )
1997 {
1998 return sgdSquare ( a[0]-b[0] ) + sgdSquare ( a[1]-b[1] ) +
1999 sgdSquare ( a[2]-b[2] ) + sgdSquare ( a[3]-b[3] ) ;
2000 }
2001
2002
sgdDistanceVec2(const sgdVec2 a,const sgdVec2 b)2003 inline SGDfloat sgdDistanceVec2 ( const sgdVec2 a, const sgdVec2 b )
2004 {
2005 return sgdSqrt ( sgdSquare ( a[0]-b[0] ) + sgdSquare ( a[1]-b[1] ) ) ;
2006 }
2007
sgdDistanceVec3(const sgdVec3 a,const sgdVec3 b)2008 inline SGDfloat sgdDistanceVec3 ( const sgdVec3 a, const sgdVec3 b )
2009 {
2010 return sgdSqrt ( sgdSquare ( a[0]-b[0] ) + sgdSquare ( a[1]-b[1] ) +
2011 sgdSquare ( a[2]-b[2] ) ) ;
2012 }
2013
sgdDistanceVec4(const sgdVec4 a,const sgdVec4 b)2014 inline SGDfloat sgdDistanceVec4 ( const sgdVec4 a, const sgdVec4 b )
2015 {
2016 return sgdSqrt ( sgdSquare ( a[0]-b[0] ) + sgdSquare ( a[1]-b[1] ) +
2017 sgdSquare ( a[2]-b[2] ) + sgdSquare ( a[3]-b[3] ) ) ;
2018 }
2019
2020
sgdLengthVec2(sgdVec2 const src)2021 inline SGDfloat sgdLengthVec2 ( sgdVec2 const src )
2022 {
2023 return sgdSqrt ( sgdScalarProductVec2 ( src, src ) ) ;
2024 }
2025
sgdLengthVec3(sgdVec3 const src)2026 inline SGDfloat sgdLengthVec3 ( sgdVec3 const src )
2027 {
2028 return sgdSqrt ( sgdScalarProductVec3 ( src, src ) ) ;
2029 }
2030
sgdLengthVec4(sgdVec4 const src)2031 inline SGDfloat sgdLengthVec4 ( sgdVec4 const src )
2032 {
2033 return sgdSqrt ( sgdScalarProductVec4 ( src, src ) ) ;
2034 }
2035
sgdLengthSquaredVec2(sgdVec2 const src)2036 inline SGDfloat sgdLengthSquaredVec2 ( sgdVec2 const src )
2037 {
2038 return sgdScalarProductVec2 ( src, src ) ;
2039 }
2040
sgdLengthSquaredVec3(sgdVec3 const src)2041 inline SGDfloat sgdLengthSquaredVec3 ( sgdVec3 const src )
2042 {
2043 return sgdScalarProductVec3 ( src, src ) ;
2044 }
2045
sgdLengthSquaredVec4(sgdVec4 const src)2046 inline SGDfloat sgdLengthSquaredVec4 ( sgdVec4 const src )
2047 {
2048 return sgdScalarProductVec4 ( src, src ) ;
2049 }
2050
2051 /* Anglo-US spelling issues. <sigh> */
2052 #define sgdNormalizeVec2 sgdNormaliseVec2
2053 #define sgdNormalizeVec3 sgdNormaliseVec3
2054 #define sgdNormalizeVec4 sgdNormaliseVec4
2055 #define sgdNormalizeQuat sgdNormaliseQuat
2056
sgdNormaliseVec2(sgdVec2 dst)2057 inline void sgdNormaliseVec2 ( sgdVec2 dst )
2058 {
2059 sgdScaleVec2 ( dst, SGD_ONE / sgdLengthVec2 ( dst ) ) ;
2060 }
2061
sgdNormaliseVec3(sgdVec3 dst)2062 inline void sgdNormaliseVec3 ( sgdVec3 dst )
2063 {
2064 sgdScaleVec3 ( dst, SGD_ONE / sgdLengthVec3 ( dst ) ) ;
2065 }
2066
sgdNormaliseVec4(sgdVec4 dst)2067 inline void sgdNormaliseVec4 ( sgdVec4 dst )
2068 {
2069 sgdScaleVec4 ( dst, SGD_ONE / sgdLengthVec4 ( dst ) ) ;
2070 }
2071
sgdNormaliseVec2(sgdVec2 dst,const sgdVec2 src)2072 inline void sgdNormaliseVec2 ( sgdVec2 dst, const sgdVec2 src )
2073 {
2074 sgdScaleVec2 ( dst, src, SGD_ONE / sgdLengthVec2 ( src ) ) ;
2075 }
2076
sgdNormaliseVec3(sgdVec3 dst,const sgdVec3 src)2077 inline void sgdNormaliseVec3 ( sgdVec3 dst, const sgdVec3 src )
2078 {
2079 sgdScaleVec3 ( dst, src, SGD_ONE / sgdLengthVec3 ( src ) ) ;
2080 }
2081
sgdNormaliseVec4(sgdVec4 dst,const sgdVec4 src)2082 inline void sgdNormaliseVec4 ( sgdVec4 dst, const sgdVec4 src )
2083 {
2084 sgdScaleVec4 ( dst, src, SGD_ONE / sgdLengthVec4 ( src ) ) ;
2085 }
2086
2087
sgdZeroCoord(sgdCoord * dst)2088 inline void sgdZeroCoord ( sgdCoord *dst )
2089 {
2090 sgdSetVec3 ( dst->xyz, SGD_ZERO, SGD_ZERO, SGD_ZERO ) ;
2091 sgdSetVec3 ( dst->hpr, SGD_ZERO, SGD_ZERO, SGD_ZERO ) ;
2092 }
2093
sgdSetCoord(sgdCoord * dst,const SGDfloat x,const SGDfloat y,const SGDfloat z,const SGDfloat h,const SGDfloat p,const SGDfloat r)2094 inline void sgdSetCoord ( sgdCoord *dst, const SGDfloat x, const SGDfloat y, const SGDfloat z,
2095 const SGDfloat h, const SGDfloat p, const SGDfloat r )
2096 {
2097 sgdSetVec3 ( dst->xyz, x, y, z ) ;
2098 sgdSetVec3 ( dst->hpr, h, p, r ) ;
2099 }
2100
sgdSetCoord(sgdCoord * dst,const sgdVec3 xyz,const sgdVec3 hpr)2101 inline void sgdSetCoord ( sgdCoord *dst, const sgdVec3 xyz, const sgdVec3 hpr )
2102 {
2103 sgdCopyVec3 ( dst->xyz, xyz ) ;
2104 sgdCopyVec3 ( dst->hpr, hpr ) ;
2105 }
2106
sgdCopyCoord(sgdCoord * dst,const sgdCoord * src)2107 inline void sgdCopyCoord ( sgdCoord *dst, const sgdCoord *src )
2108 {
2109 sgdCopyVec3 ( dst->xyz, src->xyz ) ;
2110 sgdCopyVec3 ( dst->hpr, src->hpr ) ;
2111 }
2112
2113
2114
sgdCopyMat4(sgdMat4 dst,const sgdMat4 src)2115 inline void sgdCopyMat4 ( sgdMat4 dst, const sgdMat4 src )
2116 {
2117 sgdCopyVec4 ( dst[ 0 ], src[ 0 ] ) ;
2118 sgdCopyVec4 ( dst[ 1 ], src[ 1 ] ) ;
2119 sgdCopyVec4 ( dst[ 2 ], src[ 2 ] ) ;
2120 sgdCopyVec4 ( dst[ 3 ], src[ 3 ] ) ;
2121 }
2122
2123
sgdScaleMat4(sgdMat4 dst,const sgdMat4 src,const SGDfloat scale)2124 inline void sgdScaleMat4 ( sgdMat4 dst, const sgdMat4 src, const SGDfloat scale )
2125 {
2126 sgdScaleVec4 ( dst[0], src[0], scale ) ;
2127 sgdScaleVec4 ( dst[1], src[1], scale ) ;
2128 sgdScaleVec4 ( dst[2], src[2], scale ) ;
2129 sgdScaleVec4 ( dst[3], src[3], scale ) ;
2130 }
2131
2132
sgdMakeIdentMat4(sgdMat4 dst)2133 inline void sgdMakeIdentMat4 ( sgdMat4 dst )
2134 {
2135 sgdSetVec4 ( dst[0], SGD_ONE , SGD_ZERO, SGD_ZERO, SGD_ZERO ) ;
2136 sgdSetVec4 ( dst[1], SGD_ZERO, SGD_ONE , SGD_ZERO, SGD_ZERO ) ;
2137 sgdSetVec4 ( dst[2], SGD_ZERO, SGD_ZERO, SGD_ONE , SGD_ZERO ) ;
2138 sgdSetVec4 ( dst[3], SGD_ZERO, SGD_ZERO, SGD_ZERO, SGD_ONE ) ;
2139 }
2140
2141
2142 extern void sgdMakeTransMat4 ( sgdMat4 m, const SGDfloat x, const SGDfloat y, const SGDfloat z ) ;
2143 extern void sgdMakeTransMat4 ( sgdMat4 m, const sgdVec3 xyz ) ;
2144 extern void sgdMakeCoordMat4 ( sgdMat4 m, const SGDfloat x, const SGDfloat y, const SGDfloat z,
2145 const SGDfloat h, const SGDfloat p, const SGDfloat r ) ;
2146 extern void sgdMakeCoordMat4 ( sgdMat4 m, const sgdCoord *c ) ;
2147
2148 extern int sgdCompare3DSqdDist ( const sgdVec3 a, const sgdVec3 b, const SGDfloat sqd_dist ) ;
2149
sgdDistToLineVec2(const sgdVec3 line,const sgdVec2 pnt)2150 inline SGDfloat sgdDistToLineVec2 ( const sgdVec3 line, const sgdVec2 pnt )
2151 {
2152 return sgdScalarProductVec2 ( line, pnt ) + line[2] ;
2153 }
2154
2155
2156 struct sgdLineSegment3 /* Bounded line segment */
2157 {
2158 sgdVec3 a ;
2159 sgdVec3 b ;
2160 } ;
2161
2162 struct sgdLine3 /* Infinite line */
2163 {
2164 sgdVec3 point_on_line ;
2165 sgdVec3 direction_vector ; /* Should be a unit vector */
2166 } ;
2167
2168
sgdLineSegment3ToLine3(sgdLine3 * line,const sgdLineSegment3 lineseg)2169 inline void sgdLineSegment3ToLine3 ( sgdLine3 *line,
2170 const sgdLineSegment3 lineseg )
2171 {
2172 sgdCopyVec3 ( line->point_on_line , lineseg.a ) ;
2173 sgdSubVec3 ( line->direction_vector, lineseg.b, lineseg.a ) ;
2174 sgdNormaliseVec3 ( line->direction_vector ) ;
2175 }
2176
2177
2178 SGDfloat sgdDistSquaredToLineVec3 ( const sgdLine3 line,
2179 const sgdVec3 pnt ) ;
2180 SGDfloat sgdDistSquaredToLineSegmentVec3 ( const sgdLineSegment3 line,
2181 const sgdVec3 pnt ) ;
2182
2183
sgdDistToLineVec3(const sgdLine3 line,const sgdVec3 pnt)2184 inline SGDfloat sgdDistToLineVec3 ( const sgdLine3 line,
2185 const sgdVec3 pnt )
2186 {
2187 return sgdSqrt ( sgdDistSquaredToLineVec3 ( line, pnt ) );
2188 }
2189
2190
sgdDistToLineSegmentVec3(const sgdLineSegment3 line,const sgdVec3 pnt)2191 inline SGDfloat sgdDistToLineSegmentVec3 ( const sgdLineSegment3 line,
2192 const sgdVec3 pnt )
2193 {
2194 return sgdSqrt ( sgdDistSquaredToLineSegmentVec3(line,pnt) ) ;
2195 }
2196
sgdDistToPlaneVec3(const sgdVec4 plane,const sgdVec3 pnt)2197 inline SGDfloat sgdDistToPlaneVec3 ( const sgdVec4 plane, const sgdVec3 pnt )
2198 {
2199 return sgdScalarProductVec3 ( plane, pnt ) + plane[3] ;
2200 }
2201
sgdHeightAbovePlaneVec3(const sgdVec4 plane,const sgdVec3 pnt)2202 inline SGDfloat sgdHeightAbovePlaneVec3 ( const sgdVec4 plane, const sgdVec3 pnt )
2203 {
2204 return pnt[2] - sgdHeightOfPlaneVec2 ( plane, pnt ) ;
2205 }
2206
2207 extern void sgdReflectInPlaneVec3 ( sgdVec3 dst, const sgdVec3 src, const sgdVec4 plane ) ;
2208
sgdReflectInPlaneVec3(sgdVec3 dst,const sgdVec4 plane)2209 inline void sgdReflectInPlaneVec3 ( sgdVec3 dst, const sgdVec4 plane )
2210 {
2211 sgdReflectInPlaneVec3 ( dst, dst, plane ) ;
2212 }
2213
2214 extern void sgdMakeNormal ( sgdVec3 dst, const sgdVec3 a, const sgdVec3 b, const sgdVec3 c ) ;
2215
sgdMake2DLine(sgdVec3 dst,const sgdVec2 a,const sgdVec2 b)2216 inline void sgdMake2DLine ( sgdVec3 dst, const sgdVec2 a, const sgdVec2 b )
2217 {
2218 dst[0] = b[1]-a[1] ;
2219 dst[1] = -b[0]-a[0] ;
2220 sgdNormalizeVec2 ( dst ) ;
2221 dst[2] = - ( dst[0]*a[0] + dst[1]*a[1] ) ;
2222 }
2223
sgdMakePlane(sgdVec4 dst,const sgdVec3 normal,const sgdVec3 pnt)2224 inline void sgdMakePlane ( sgdVec4 dst, const sgdVec3 normal, const sgdVec3 pnt )
2225 {
2226 sgdCopyVec3 ( dst, normal ) ;
2227 dst [ 3 ] = - sgdScalarProductVec3 ( normal, pnt ) ;
2228 }
2229
sgdMakePlane(sgdVec4 dst,const sgdVec3 a,const sgdVec3 b,const sgdVec3 c)2230 inline void sgdMakePlane ( sgdVec4 dst, const sgdVec3 a, const sgdVec3 b, const sgdVec3 c )
2231 {
2232 /*
2233 Ax + By + Cz + D == 0 ;
2234 D = - ( Ax + By + Cz )
2235 = - ( A*a[0] + B*a[1] + C*a[2] )
2236 = - sgdScalarProductVec3 ( normal, a ) ;
2237 */
2238
2239 sgdMakeNormal ( dst, a, b, c ) ;
2240
2241 dst [ 3 ] = - sgdScalarProductVec3 ( dst, a ) ;
2242 }
2243
2244 SGDfloat sgdTriArea( sgdVec3 p0, sgdVec3 p1, sgdVec3 p2 );
2245
2246
2247 // Fast code. Result is in the range 0..180:
sgdAngleBetweenNormalizedVec3(sgdVec3 v1,sgdVec3 v2)2248 inline SGDfloat sgdAngleBetweenNormalizedVec3 ( sgdVec3 v1, sgdVec3 v2 )
2249 {
2250 SGDfloat f = sgdScalarProductVec3 ( v1, v2 ) ;
2251
2252 return (float) acos ( ( f >= 1.0f ) ? 1.0f :
2253 ( f <= -1.0f ) ? -1.0f : f ) *
2254 SGD_RADIANS_TO_DEGREES ;
2255 }
2256
2257 // Fast code. Result is in the range 0..180:
2258
2259 SGDfloat sgdAngleBetweenVec3 ( sgdVec3 v1, sgdVec3 v2 );
2260
2261 // All three have to be normalized. Slow code. Result is in the range 0..360:
2262
2263 SGDfloat sgdAngleBetweenNormalizedVec3 (sgdVec3 first, sgdVec3 second, sgdVec3 normal);
2264
2265 // Normal has to be normalized. Slow code. Result is in the range 0..360:
2266
2267 SGDfloat sgdAngleBetweenVec3 ( sgdVec3 v1, sgdVec3 v2, sgdVec3 normal );
2268
2269
2270
2271 class sgdSphere
2272 {
2273 public:
2274 sgdVec3 center ;
2275 SGDfloat radius ;
2276
2277
getCenter(void)2278 const SGDfloat *getCenter (void) const { return center ; }
2279
setCenter(const sgdVec3 c)2280 void setCenter ( const sgdVec3 c )
2281 {
2282 sgdCopyVec3 ( center, c ) ;
2283 }
2284
setCenter(const SGDfloat x,const SGDfloat y,const SGDfloat z)2285 void setCenter ( const SGDfloat x, const SGDfloat y, const SGDfloat z )
2286 {
2287 sgdSetVec3 ( center, x, y, z ) ;
2288 }
2289
getRadius(void)2290 SGDfloat getRadius (void) const { return radius ; }
setRadius(const SGDfloat r)2291 void setRadius ( const SGDfloat r ) { radius = r ; }
2292
isEmpty(void)2293 int isEmpty (void) const { return radius < SGD_ZERO ; }
empty(void)2294 void empty (void) { radius = - SGD_ONE ; }
2295
orthoXform(const sgdMat4 m)2296 void orthoXform ( const sgdMat4 m )
2297 {
2298 sgdXformPnt3 ( center, center, m ) ;
2299 // radius *= sgdLengthVec3 ( m[0] ) ;
2300 }
2301
2302 void extend ( const sgdSphere *s ) ;
2303 void extend ( const sgdBox *b ) ;
2304 void extend ( const sgdVec3 v ) ;
2305
intersects(const sgdSphere * s)2306 int intersects ( const sgdSphere *s ) const
2307 {
2308 return sgdCompare3DSqdDist ( center, s->getCenter(),
2309 sgdSquare ( radius + s->getRadius() ) ) <= 0 ;
2310 }
2311
intersects(const sgdVec4 plane)2312 int intersects ( const sgdVec4 plane ) const
2313 {
2314 return sgdAbs ( sgdDistToPlaneVec3 ( plane, center ) ) <= radius ;
2315 }
2316
2317 int intersects ( const sgdBox *b ) const ;
2318 } ;
2319
2320
2321 class sgdBox
2322 {
2323 public:
2324 sgdVec3 min ;
2325 sgdVec3 max ;
2326
2327
getMin(void)2328 const SGDfloat *getMin (void) const { return min ; }
getMax(void)2329 const SGDfloat *getMax (void) const { return max ; }
2330
setMin(const SGDfloat x,const SGDfloat y,const SGDfloat z)2331 void setMin ( const SGDfloat x, const SGDfloat y, const SGDfloat z )
2332 {
2333 sgdSetVec3 ( min, x, y, z ) ;
2334 }
2335
setMin(const sgdVec3 src)2336 void setMin ( const sgdVec3 src )
2337 {
2338 sgdCopyVec3 ( min, src ) ;
2339 }
2340
setMax(const SGDfloat x,const SGDfloat y,const SGDfloat z)2341 void setMax ( const SGDfloat x, const SGDfloat y, const SGDfloat z )
2342 {
2343 sgdSetVec3 ( max, x, y, z ) ;
2344 }
2345
setMax(const sgdVec3 src)2346 void setMax ( const sgdVec3 src )
2347 {
2348 sgdCopyVec3 ( max, src ) ;
2349 }
2350
isEmpty(void)2351 int isEmpty(void) const
2352 {
2353 return ( min[0] > max[0] ||
2354 min[1] > max[1] ||
2355 min[2] > max[2] ) ;
2356 }
2357
empty(void)2358 void empty (void)
2359 {
2360 sgdSetVec3 ( min, SGD_MAX, SGD_MAX, SGD_MAX ) ;
2361 sgdSetVec3 ( max, -SGD_MAX, -SGD_MAX, -SGD_MAX ) ;
2362 }
2363
2364 void extend ( const sgdSphere *s ) ;
2365 void extend ( const sgdBox *b ) ;
2366 void extend ( const sgdVec3 v ) ;
2367
intersects(const sgdSphere * s)2368 int intersects ( const sgdSphere *s ) const
2369 {
2370 return s -> intersects ( this ) ;
2371 }
2372
intersects(const sgdBox * b)2373 int intersects ( const sgdBox *b ) const
2374 {
2375 return min[0] <= b->getMax()[0] && max[0] >= b->getMin()[0] &&
2376 min[1] <= b->getMax()[1] && max[1] >= b->getMin()[1] &&
2377 min[2] <= b->getMax()[2] && max[2] >= b->getMin()[2] ;
2378 }
2379
2380 int intersects ( const sgdVec4 plane ) const ;
2381 } ;
2382
2383
2384 class sgdFrustum
2385 {
2386 /* Is the projection orthographic (or perspective)? */
2387 int ortho ;
2388
2389 /* The parameters for glFrustum/glOrtho */
2390
2391 SGDfloat left, right, bot, top, nnear, ffar ;
2392
2393 /* The computed projection matrix for this frustum */
2394
2395 sgdMat4 mat ;
2396
2397 /* The A,B,C,D terms of the plane equations of the clip planes */
2398 /* A point (x,y,z) is inside the frustum iff Ax + By + Cz + D >= 0 for all planes */
2399
2400 sgdVec4 plane [ 6 ] ;
2401
2402 /* These two are only valid for simple frusta */
2403
2404 SGDfloat hfov ; /* Horizontal Field of View -or- Orthographic Width */
2405 SGDfloat vfov ; /* Vertical Field of View -or- Orthographic Height */
2406
2407 void update (void) ;
2408 int getOutcode ( const sgdVec4 src ) const ;
2409
2410 public:
2411
sgdFrustum(void)2412 sgdFrustum (void)
2413 {
2414 ortho = FALSE ;
2415 nnear = SGD_ONE ;
2416 ffar = 1000000.0f ;
2417 hfov = SGD_45 ;
2418 vfov = SGD_45 ;
2419 update () ;
2420 }
2421
setFrustum(const SGDfloat l,const SGDfloat r,const SGDfloat b,const SGDfloat t,const SGDfloat n,const SGDfloat f)2422 void setFrustum ( const SGDfloat l, const SGDfloat r,
2423 const SGDfloat b, const SGDfloat t,
2424 const SGDfloat n, const SGDfloat f )
2425 {
2426 ortho = FALSE ;
2427 left = l ;
2428 right = r ;
2429 bot = b ;
2430 top = t ;
2431 nnear = n ;
2432 ffar = f ;
2433 hfov = vfov = SGD_ZERO ;
2434 update () ;
2435 }
2436
setOrtho(const SGDfloat l,const SGDfloat r,const SGDfloat b,const SGDfloat t,const SGDfloat n,const SGDfloat f)2437 void setOrtho ( const SGDfloat l, const SGDfloat r,
2438 const SGDfloat b, const SGDfloat t,
2439 const SGDfloat n, const SGDfloat f )
2440 {
2441 ortho = TRUE ;
2442 left = l ;
2443 right = r ;
2444 bot = b ;
2445 top = t ;
2446 nnear = n ;
2447 ffar = f ;
2448 hfov = vfov = SGD_ZERO ;
2449 update () ;
2450 }
2451
getMat4(sgdMat4 dst)2452 void getMat4 ( sgdMat4 dst ) { sgdCopyMat4 ( dst, mat ) ; }
2453
getLeft(void)2454 SGDfloat getLeft (void) const { return left ; }
getRight(void)2455 SGDfloat getRight(void) const { return right ; }
getBot(void)2456 SGDfloat getBot (void) const { return bot ; }
getTop(void)2457 SGDfloat getTop (void) const { return top ; }
getNear(void)2458 SGDfloat getNear (void) const { return nnear ; }
getFar(void)2459 SGDfloat getFar (void) const { return ffar ; }
2460
getPlane(int i)2461 const SGDfloat *getPlane ( int i ) const { return plane [ i ] ; }
2462
getHFOV(void)2463 SGDfloat getHFOV (void) const { return hfov ; }
getVFOV(void)2464 SGDfloat getVFOV (void) const { return vfov ; }
2465
getFOV(SGDfloat * h,SGDfloat * v)2466 void getFOV ( SGDfloat *h, SGDfloat *v ) const
2467 {
2468 if ( h != (SGDfloat *) 0 ) *h = hfov ;
2469 if ( v != (SGDfloat *) 0 ) *v = vfov ;
2470 }
2471
setFOV(const SGDfloat h,const SGDfloat v)2472 void setFOV ( const SGDfloat h, const SGDfloat v )
2473 {
2474 ortho = FALSE ;
2475 hfov = ( h <= 0 ) ? ( v * SGD_FOUR / SGD_THREE ) : h ;
2476 vfov = ( v <= 0 ) ? ( h * SGD_THREE / SGD_FOUR ) : v ;
2477 update () ;
2478 }
2479
getOrtho(SGDfloat * w,SGDfloat * h)2480 void getOrtho ( SGDfloat *w, SGDfloat *h ) const
2481 {
2482 if ( w != (SGDfloat *) 0 ) *w = right - left ;
2483 if ( h != (SGDfloat *) 0 ) *h = top - bot ;
2484 }
2485
setOrtho(const SGDfloat w,const SGDfloat h)2486 void setOrtho ( const SGDfloat w, const SGDfloat h )
2487 {
2488 ortho = TRUE ;
2489 hfov = ( w <= 0 ) ? ( h * SGD_FOUR / SGD_THREE ) : w ;
2490 vfov = ( h <= 0 ) ? ( w * SGD_FOUR / SGD_THREE ) : h ;
2491 update () ;
2492 }
2493
getNearFar(SGDfloat * n,SGDfloat * f)2494 void getNearFar ( SGDfloat *n, SGDfloat *f ) const
2495 {
2496 if ( n != (SGDfloat *) 0 ) *n = nnear ;
2497 if ( f != (SGDfloat *) 0 ) *f = ffar ;
2498 }
2499
setNearFar(const SGDfloat n,const SGDfloat f)2500 void setNearFar ( const SGDfloat n, const SGDfloat f )
2501 {
2502 nnear = n ;
2503 ffar = f ;
2504 update () ;
2505 }
2506
isOrtho(void)2507 int isOrtho (void) const { return ortho ; }
2508
2509 int contains ( const sgdVec3 p ) const ;
2510 int contains ( const sgdSphere *s ) const ;
2511 int contains ( const sgdBox *b ) const ;
2512 } ;
2513
2514
2515 /*
2516 Quaternion routines are Copyright (C) 1999
2517 Kevin B. Thompson <kevinbthompson@yahoo.com>
2518 Modified by Sylvan W. Clebsch <sylvan@stanford.edu>
2519 Largely rewritten by "Negative0" <negative0@earthlink.net>
2520 */
2521
2522 /*
2523 Quaternion structure w = real, (x, y, z) = vector
2524 CHANGED sqQuat to float array so that syntax matches
2525 vector and matrix routines
2526 */
2527
2528
sgdMakeIdentQuat(sgdQuat dst)2529 inline void sgdMakeIdentQuat ( sgdQuat dst )
2530 {
2531 sgdSetVec4 ( dst, SGD_ZERO, SGD_ZERO, SGD_ZERO, SGD_ONE ) ;
2532 }
2533
2534
sgdSetQuat(sgdQuat dst,const SGDfloat w,const SGDfloat x,const SGDfloat y,const SGDfloat z)2535 inline void sgdSetQuat ( sgdQuat dst,
2536 const SGDfloat w, const SGDfloat x,
2537 const SGDfloat y, const SGDfloat z )
2538 {
2539 sgdSetVec4 ( dst, x, y, z, w ) ;
2540 }
2541
sgdCopyQuat(sgdQuat dst,const sgdQuat src)2542 inline void sgdCopyQuat ( sgdQuat dst, const sgdQuat src )
2543 {
2544 sgdCopyVec4 ( dst, src ) ;
2545 }
2546
2547
2548 /* Construct a unit quaternion (length==1) */
2549
sgdNormaliseQuat(sgdQuat dst,const sgdQuat src)2550 inline void sgdNormaliseQuat ( sgdQuat dst, const sgdQuat src )
2551 {
2552 SGDfloat d = sgdScalarProductVec4 ( src, src ) ;
2553
2554 d = (d > SGD_ZERO) ? (SGD_ONE / sgdSqrt ( d )) : SGD_ONE ;
2555
2556 sgdScaleVec4 ( dst, src, d ) ;
2557 }
2558
2559
2560
sgdNormaliseQuat(sgdQuat dst)2561 inline void sgdNormaliseQuat ( sgdQuat dst ) { sgdNormaliseQuat ( dst, dst ) ; }
2562
2563
sgdInvertQuat(sgdQuat dst,const sgdQuat src)2564 inline void sgdInvertQuat ( sgdQuat dst, const sgdQuat src )
2565 {
2566 SGDfloat d = sgdScalarProductVec4 ( src, src ) ;
2567
2568 d = ( d == SGD_ZERO ) ? SGD_ONE : ( SGD_ONE / d ) ;
2569
2570 dst[SG_W] = src[SG_W] * d ;
2571 dst[SG_X] = -src[SG_X] * d ;
2572 dst[SG_Y] = -src[SG_Y] * d ;
2573 dst[SG_Z] = -src[SG_Z] * d ;
2574 }
2575
sgdInvertQuat(sgdQuat dst)2576 inline void sgdInvertQuat ( sgdQuat dst ) { sgdInvertQuat ( dst, dst ) ; }
2577
2578
2579 /* Make an angle and axis of rotation from a Quaternion. */
2580
2581 void sgdQuatToAngleAxis ( SGDfloat *angle, sgdVec3 axis, const sgdQuat src ) ;
2582 void sgdQuatToAngleAxis ( SGDfloat *angle,
2583 SGDfloat *x, SGDfloat *y, SGDfloat *z,
2584 const sgdQuat src ) ;
2585
2586 /* Make a quaternion from a given angle and axis of rotation */
2587
2588 void sgdAngleAxisToQuat ( sgdQuat dst,
2589 const SGDfloat angle, const sgdVec3 axis ) ;
2590 void sgdAngleAxisToQuat ( sgdQuat dst,
2591 const SGDfloat angle,
2592 const SGDfloat x, const SGDfloat y, const SGDfloat z );
2593
2594 /* Convert a matrix to/from a quat */
2595
2596 void sgdMatrixToQuat ( sgdQuat quat, const sgdMat4 m ) ;
2597 void sgdQuatToMatrix ( sgdMat4 m, const sgdQuat quat ) ;
2598
2599 /* Convert a set of eulers to/from a quat */
2600
2601 void sgdQuatToEuler( sgdVec3 hpr, const sgdQuat quat ) ;
2602 void sgdEulerToQuat( sgdQuat quat, const sgdVec3 hpr ) ;
2603
sgdEulerToQuat(sgdQuat dst,SGDfloat h,SGDfloat p,SGDfloat r)2604 inline void sgdEulerToQuat( sgdQuat dst,
2605 SGDfloat h, SGDfloat p, SGDfloat r )
2606 {
2607 sgdVec3 hpr ;
2608
2609 sgdSetVec3 ( hpr, h, p, r ) ;
2610
2611 sgdEulerToQuat( dst, hpr ) ;
2612 }
2613
sgdHPRToQuat(sgdQuat dst,SGDfloat h,SGDfloat p,SGDfloat r)2614 inline void sgdHPRToQuat ( sgdQuat dst, SGDfloat h, SGDfloat p, SGDfloat r )
2615 {
2616 sgdVec3 hpr;
2617
2618 hpr[0] = h * SGD_DEGREES_TO_RADIANS ;
2619 hpr[1] = p * SGD_DEGREES_TO_RADIANS ;
2620 hpr[2] = r * SGD_DEGREES_TO_RADIANS ;
2621
2622 sgdEulerToQuat( dst, hpr ) ;
2623 }
2624
sgdHPRToQuat(sgdQuat dst,const sgdVec3 hpr)2625 inline void sgdHPRToQuat ( sgdQuat dst, const sgdVec3 hpr )
2626 {
2627 sgdVec3 tmp ;
2628
2629 sgdScaleVec3 ( tmp, hpr, SGD_DEGREES_TO_RADIANS ) ;
2630
2631 sgdEulerToQuat ( dst, tmp ) ;
2632 }
2633
2634 /* Multiply quaternions together (concatenate rotations) */
2635
2636 void sgdMultQuat ( sgdQuat dst, const sgdQuat a, const sgdQuat b ) ;
2637
sgdPostMultQuat(sgdQuat dst,const sgdQuat q)2638 inline void sgdPostMultQuat ( sgdQuat dst, const sgdQuat q )
2639 {
2640 sgdQuat r ;
2641
2642 sgdCopyQuat ( r, dst ) ;
2643 sgdMultQuat ( dst, r, q ) ;
2644 }
2645
sgdPreMultQuat(sgdQuat dst,const sgdQuat q)2646 inline void sgdPreMultQuat ( sgdQuat dst, const sgdQuat q )
2647 {
2648 sgdQuat r ;
2649
2650 sgdCopyQuat ( r, dst ) ;
2651 sgdMultQuat ( dst, q, r ) ;
2652 }
2653
2654
2655 /* Rotate a quaternion by a given angle and axis (convenience function) */
2656
2657
sgdRotQuat(sgdQuat dst,const SGDfloat angle,const sgdVec3 axis)2658 inline void sgdRotQuat ( sgdQuat dst, const SGDfloat angle, const sgdVec3 axis )
2659 {
2660 sgdQuat q ;
2661
2662 sgdAngleAxisToQuat ( q, angle, axis ) ;
2663 sgdPostMultQuat ( dst, q ) ;
2664 sgdNormaliseQuat ( dst ) ;
2665 }
2666
2667
sgdRotQuat(sgdQuat dst,const SGDfloat angle,const SGDfloat x,const SGDfloat y,const SGDfloat z)2668 inline void sgdRotQuat ( sgdQuat dst,
2669 const SGDfloat angle,
2670 const SGDfloat x, const SGDfloat y, const SGDfloat z )
2671 {
2672 sgdVec3 axis ;
2673
2674 sgdSetVec3 ( axis, x, y, z ) ;
2675 sgdRotQuat ( dst, angle, axis ) ;
2676 }
2677
2678 /* SWC - Interpolate between to quaternions */
2679
2680 extern void sgdSlerpQuat ( sgdQuat dst,
2681 const sgdQuat from, const sgdQuat to,
2682 const SGDfloat t ) ;
2683
2684
2685 /* Conversions between sg and sgd types. */
2686
sgSetVec2(sgVec2 dst,sgdVec2 src)2687 inline void sgSetVec2 ( sgVec2 dst, sgdVec2 src )
2688 {
2689 dst [ 0 ] = (SGfloat) src [ 0 ] ;
2690 dst [ 1 ] = (SGfloat) src [ 1 ] ;
2691 }
2692
sgSetVec3(sgVec3 dst,sgdVec3 src)2693 inline void sgSetVec3 ( sgVec3 dst, sgdVec3 src )
2694 {
2695 dst [ 0 ] = (SGfloat) src [ 0 ] ;
2696 dst [ 1 ] = (SGfloat) src [ 1 ] ;
2697 dst [ 2 ] = (SGfloat) src [ 2 ] ;
2698 }
2699
sgSetVec4(sgVec4 dst,sgdVec4 src)2700 inline void sgSetVec4 ( sgVec4 dst, sgdVec4 src )
2701 {
2702 dst [ 0 ] = (SGfloat) src [ 0 ] ;
2703 dst [ 1 ] = (SGfloat) src [ 1 ] ;
2704 dst [ 2 ] = (SGfloat) src [ 2 ] ;
2705 dst [ 3 ] = (SGfloat) src [ 3 ] ;
2706 }
2707
sgdSetVec2(sgdVec2 dst,sgVec2 src)2708 inline void sgdSetVec2 ( sgdVec2 dst, sgVec2 src )
2709 {
2710 dst [ 0 ] = (SGDfloat) src [ 0 ] ;
2711 dst [ 1 ] = (SGDfloat) src [ 1 ] ;
2712 }
2713
sgdSetVec3(sgdVec3 dst,sgVec3 src)2714 inline void sgdSetVec3 ( sgdVec3 dst, sgVec3 src )
2715 {
2716 dst [ 0 ] = (SGDfloat) src [ 0 ] ;
2717 dst [ 1 ] = (SGDfloat) src [ 1 ] ;
2718 dst [ 2 ] = (SGDfloat) src [ 2 ] ;
2719 }
2720
sgdSetVec4(sgdVec4 dst,sgVec4 src)2721 inline void sgdSetVec4 ( sgdVec4 dst, sgVec4 src )
2722 {
2723 dst [ 0 ] = (SGDfloat) src [ 0 ] ;
2724 dst [ 1 ] = (SGDfloat) src [ 1 ] ;
2725 dst [ 2 ] = (SGDfloat) src [ 2 ] ;
2726 dst [ 3 ] = (SGDfloat) src [ 3 ] ;
2727 }
2728
2729
sgSetMat4(sgMat4 dst,sgdMat4 src)2730 inline void sgSetMat4 ( sgMat4 dst, sgdMat4 src )
2731 {
2732 sgSetVec4 ( dst [ 0 ], src [ 0 ] ) ;
2733 sgSetVec4 ( dst [ 1 ], src [ 1 ] ) ;
2734 sgSetVec4 ( dst [ 2 ], src [ 2 ] ) ;
2735 sgSetVec4 ( dst [ 3 ], src [ 3 ] ) ;
2736 }
2737
2738
sgdSetMat4(sgdMat4 dst,sgMat4 src)2739 inline void sgdSetMat4 ( sgdMat4 dst, sgMat4 src )
2740 {
2741 sgdSetVec4 ( dst [ 0 ], src [ 0 ] ) ;
2742 sgdSetVec4 ( dst [ 1 ], src [ 1 ] ) ;
2743 sgdSetVec4 ( dst [ 2 ], src [ 2 ] ) ;
2744 sgdSetVec4 ( dst [ 3 ], src [ 3 ] ) ;
2745 }
2746
2747
sgSetCoord(sgCoord * dst,sgdCoord * src)2748 inline void sgSetCoord ( sgCoord *dst, sgdCoord *src )
2749 {
2750 sgSetVec3 ( dst->xyz, src->xyz ) ;
2751 sgSetVec3 ( dst->hpr, src->hpr ) ;
2752 }
2753
2754
sgdSetCoord(sgdCoord * dst,sgCoord * src)2755 inline void sgdSetCoord ( sgdCoord *dst, sgCoord *src )
2756 {
2757 sgdSetVec3 ( dst->xyz, src->xyz ) ;
2758 sgdSetVec3 ( dst->hpr, src->hpr ) ;
2759 }
2760
sgSetQuat(sgQuat dst,sgdQuat src)2761 inline void sgSetQuat ( sgQuat dst, sgdQuat src )
2762 {
2763 sgSetVec4 ( dst, src ) ;
2764 }
2765
2766
2767
sgdSetQuat(sgdQuat dst,sgQuat src)2768 inline void sgdSetQuat ( sgdQuat dst, sgQuat src )
2769 {
2770 sgdSetVec4 ( dst, src ) ;
2771 }
2772
2773
2774 /* Function to rotate a vector through a given quaternion using the formula
2775 * R = Q r Q-1 -- this gives the components of a ROTATED vector in a STATIONARY
2776 * coordinate system. We assume that Q is a unit quaternion.
2777 */
2778
2779 void sgRotateVecQuat ( sgVec3 vec, sgQuat q ) ;
2780 void sgdRotateVecQuat ( sgdVec3 vec, sgdQuat q ) ;
2781
2782 /* Function to rotate a vector through a given quaternion using the formula
2783 * R = Q-1 r Q -- this gives the components of a STATIONARY vector in a ROTATED
2784 * coordinate system. We assume that Q is a unit quaternion.
2785 */
2786
2787 void sgRotateCoordQuat ( sgVec3 vec, sgQuat q ) ;
2788 void sgdRotateCoordQuat ( sgdVec3 vec, sgdQuat q ) ;
2789
2790 sgFloat sgDistSquaredToLineLineSegment ( const sgLineSegment3 seg, const sgLine3 line ) ;
2791 sgdFloat sgdDistSquaredToLineLineSegment ( const sgdLineSegment3 seg, const sgdLine3 line ) ;
2792
2793
2794
2795 /*
2796 Intersection testing.
2797 */
2798
2799 int sgdIsectPlanePlane ( sgdVec3 point, sgdVec3 dir,
2800 sgdVec4 plane1, sgdVec4 plane2 ) ;
2801 int sgdIsectInfLinePlane ( sgdVec3 dst,
2802 sgdVec3 l_org, sgdVec3 l_vec,
2803 sgdVec4 plane ) ;
2804 int sgdIsectInfLineInfLine ( sgdVec3 dst,
2805 sgdVec3 l1_org, sgdVec3 l1_vec,
2806 sgdVec3 l2_org, sgdVec3 l2_vec ) ;
2807 SGDfloat sgdIsectLinesegPlane ( sgdVec3 dst,
2808 sgdVec3 v1, sgdVec3 v2,
2809 sgdVec4 plane ) ;
2810 bool sgdPointInTriangle ( sgdVec3 point, sgdVec3 tri[3] );
2811
2812
2813 /*
2814 TRIANGLE SOLVERS - These work for any triangle.
2815
2816 SSS == Side-lengths for all three sides.
2817 SAS == Side-lengths for two sides - plus the angle between them.
2818 ASA == Two angles plus the length of the Side between them.
2819 Area == The area of the triangle.
2820 */
2821
2822 SGfloat sgTriangleSolver_ASAtoArea ( SGfloat angA, SGfloat lenB, SGfloat angC );
2823 SGfloat sgTriangleSolver_SAStoArea ( SGfloat lenA, SGfloat angB, SGfloat lenC );
2824 SGfloat sgTriangleSolver_SSStoArea ( SGfloat lenA, SGfloat lenB, SGfloat lenC );
2825 SGfloat sgTriangleSolver_SAAtoArea ( SGfloat lenA, SGfloat angB, SGfloat angA );
2826 SGfloat sgTriangleSolver_ASStoArea ( SGfloat angB, SGfloat lenA, SGfloat lenB,
2827 int angA_is_obtuse );
2828
2829 void sgTriangleSolver_SSStoAAA ( SGfloat lenA, SGfloat lenB, SGfloat lenC,
2830 SGfloat *angA, SGfloat *angB, SGfloat *angC ) ;
2831 void sgTriangleSolver_SAStoASA ( SGfloat lenA, SGfloat angB, SGfloat lenC,
2832 SGfloat *angA, SGfloat *lenB, SGfloat *angC ) ;
2833 void sgTriangleSolver_ASAtoSAS ( SGfloat angA, SGfloat lenB, SGfloat angC,
2834 SGfloat *lenA, SGfloat *angB, SGfloat *lenC ) ;
2835 void sgTriangleSolver_SAAtoASS ( SGfloat lenA, SGfloat angB, SGfloat angA,
2836 SGfloat *angC, SGfloat *lenB, SGfloat *lenC ) ;
2837 void sgTriangleSolver_ASStoSAA ( SGfloat angB, SGfloat lenA, SGfloat lenB,
2838 int angA_is_obtuse,
2839 SGfloat *lenC, SGfloat *angA, SGfloat *angC ) ;
2840
2841
2842 SGDfloat sgdTriangleSolver_ASAtoArea ( SGDfloat angA, SGDfloat lenB, SGDfloat angC );
2843 SGDfloat sgdTriangleSolver_SAStoArea ( SGDfloat lenA, SGDfloat angB, SGDfloat lenC );
2844 SGDfloat sgdTriangleSolver_SSStoArea ( SGDfloat lenA, SGDfloat lenB, SGDfloat lenC );
2845 SGDfloat sgdTriangleSolver_SAAtoArea ( SGDfloat lenA, SGDfloat angB, SGDfloat angA );
2846 SGDfloat sgdTriangleSolver_ASStoArea ( SGDfloat angB, SGDfloat lenA, SGDfloat lenB,
2847 int angA_is_obtuse );
2848
2849 void sgdTriangleSolver_SSStoAAA ( SGDfloat lenA, SGDfloat lenB, SGDfloat lenC,
2850 SGDfloat *angA, SGDfloat *angB, SGDfloat *angC ) ;
2851 void sgdTriangleSolver_SAStoASA ( SGDfloat lenA, SGDfloat angB, SGDfloat lenC,
2852 SGDfloat *angA, SGDfloat *lenB, SGDfloat *angC ) ;
2853 void sgdTriangleSolver_ASAtoSAS ( SGDfloat angA, SGDfloat lenB, SGDfloat angC,
2854 SGDfloat *lenA, SGDfloat *angB, SGDfloat *lenC ) ;
2855 void sgdTriangleSolver_SAAtoASS ( SGDfloat lenA, SGDfloat angB, SGDfloat angA,
2856 SGDfloat *angC, SGDfloat *lenB, SGDfloat *lenC ) ;
2857 void sgdTriangleSolver_ASStoSAA ( SGDfloat angB, SGDfloat lenA, SGDfloat lenB,
2858 int angA_is_obtuse,
2859 SGDfloat *lenC, SGDfloat *angA, SGDfloat *angC ) ;
2860
2861 /*
2862 SPRING-MASS-DAMPER (with simple Euler integrator)
2863 */
2864
2865
2866 extern sgVec3 _sgGravity ;
2867
sgSetGravity(float g)2868 inline void sgSetGravity ( float g ) { sgSetVec3 ( _sgGravity, 0.0f, 0.0f, -g ) ; }
sgSetGravityVec3(sgVec3 g)2869 inline void sgSetGravityVec3 ( sgVec3 g ) { sgCopyVec3 ( _sgGravity, g ) ; }
sgGetGravityVec3()2870 inline float *sgGetGravityVec3 () { return _sgGravity ; }
sgGetGravity()2871 inline float sgGetGravity () { return - _sgGravity[2] ; }
2872
2873
2874 class sgParticle
2875 {
2876 float ooMass ; /* One-over-mass */
2877 sgVec3 pos ;
2878 sgVec3 vel ;
2879 sgVec3 force ;
2880
2881 public:
2882
sgParticle(float mass,sgVec3 _pos)2883 sgParticle ( float mass, sgVec3 _pos )
2884 {
2885 setMass ( mass ) ;
2886 sgCopyVec3 ( pos, _pos ) ;
2887 sgZeroVec3 ( vel ) ;
2888 sgZeroVec3 ( force ) ;
2889 }
2890
2891 sgParticle ( float mass, float x = 0.0f, float y = 0.0f, float z = 0.0f )
2892 {
2893 setMass ( mass ) ;
2894 sgSetVec3 ( pos, x, y, z ) ;
2895 sgZeroVec3 ( vel ) ;
2896 sgZeroVec3 ( force ) ;
2897 }
2898
getPos()2899 SGfloat *getPos () { return pos ; }
getVel()2900 SGfloat *getVel () { return vel ; }
getForce()2901 SGfloat *getForce () { return force ; }
2902
getPos()2903 const SGfloat *getPos () const { return pos ; }
getVel()2904 const SGfloat *getVel () const { return vel ; }
getForce()2905 const SGfloat *getForce () const { return force ; }
2906
getOneOverMass()2907 float getOneOverMass () { return ooMass ; }
getMass()2908 float getMass () { return 1.0f / ooMass ; }
2909
setPos(sgVec3 p)2910 void setPos ( sgVec3 p ) { sgCopyVec3 ( pos , p ) ; }
setVel(sgVec3 v)2911 void setVel ( sgVec3 v ) { sgCopyVec3 ( vel , v ) ; }
setForce(sgVec3 f)2912 void setForce ( sgVec3 f ) { sgCopyVec3 ( force, f ) ; }
2913
setPos(float x,float y,float z)2914 void setPos ( float x, float y, float z ) { sgSetVec3 ( pos ,x,y,z ) ; }
setVel(float x,float y,float z)2915 void setVel ( float x, float y, float z ) { sgSetVec3 ( vel ,x,y,z ) ; }
setForce(float x,float y,float z)2916 void setForce ( float x, float y, float z ) { sgSetVec3 ( force,x,y,z ) ; }
2917
setOneOverMass(float oom)2918 void setOneOverMass ( float oom ) { ooMass = oom ; }
2919
setMass(float m)2920 void setMass ( float m )
2921 {
2922 assert ( m > 0.0f ) ;
2923 ooMass = 1.0f / m ;
2924 }
2925
zeroForce()2926 void zeroForce () { sgZeroVec3 ( force ) ; }
addForce(sgVec3 f)2927 void addForce ( sgVec3 f ) { sgAddVec3 ( force, f ) ; }
subForce(sgVec3 f)2928 void subForce ( sgVec3 f ) { sgSubVec3 ( force, f ) ; }
gravityOnly()2929 void gravityOnly () { sgScaleVec3 ( force, sgGetGravityVec3 (), ooMass ) ; }
2930
bounce(sgVec3 normal,float coefRestitution)2931 void bounce ( sgVec3 normal, float coefRestitution )
2932 {
2933 sgVec3 vn, vt ;
2934 sgScaleVec3 ( vn, normal,
2935 sgScalarProductVec3 ( normal, vel ) ) ;
2936 sgSubVec3 ( vt, vel, vn ) ;
2937 sgAddScaledVec3 ( vel, vt, vn, -coefRestitution ) ;
2938 }
2939
update(float dt)2940 void update ( float dt )
2941 {
2942 sgAddScaledVec3 ( vel, force, dt * ooMass ) ;
2943 sgAddScaledVec3 ( pos, vel, dt ) ;
2944 }
2945 } ;
2946
2947
2948 class sgSpringDamper
2949 {
2950 sgParticle *p0 ;
2951 sgParticle *p1 ;
2952
2953 float restLength ;
2954 float stiffness ;
2955 float damping ;
2956
2957 public:
2958
sgSpringDamper()2959 sgSpringDamper ()
2960 {
2961 p0 = p1 = NULL ;
2962 stiffness = 1.0f ;
2963 damping = 1.0f ;
2964 restLength = 1.0f ;
2965 }
2966
2967 sgSpringDamper ( sgParticle *_p0, sgParticle *_p1,
2968 float _stiffness, float _damping,
2969 float _restLength = -1.0f )
2970 {
2971 p0 = _p0 ;
2972 p1 = _p1 ;
2973 stiffness = _stiffness ;
2974 damping = _damping ;
2975
2976 if ( _restLength < 0.0f )
2977 {
2978 if ( p0 != NULL && p1 != NULL )
2979 restLength = sgDistanceVec3 ( p0->getPos(), p1->getPos() ) ;
2980 else
2981 restLength = _restLength ;
2982 }
2983 else
2984 restLength = 1.0f ;
2985 }
2986
getRestLength()2987 float getRestLength () { return restLength ; }
getStiffness()2988 float getStiffness () { return stiffness ; }
getDamping()2989 float getDamping () { return damping ; }
2990
getParticle(int which)2991 sgParticle *getParticle ( int which ) { return ( which == 0 ) ? p0 : p1 ; }
2992
2993
setParticles(sgParticle * _p0,sgParticle * _p1)2994 void setParticles ( sgParticle *_p0, sgParticle *_p1 ) { p0 = _p0 ; p1 = _p1 ; }
setParticle(int which,sgParticle * p)2995 void setParticle ( int which, sgParticle *p ) { if ( which == 0 ) p0 = p ; else p1 = p ; }
2996
setRestLength()2997 void setRestLength () { restLength = sgDistanceVec3 ( p0->getPos(), p1->getPos() ) ; }
2998
setRestLength(float l)2999 void setRestLength ( float l ) { restLength = l ; }
setStiffness(float s)3000 void setStiffness ( float s ) { stiffness = s ; }
setDamping(float d)3001 void setDamping ( float d ) { damping = d ; }
3002
update()3003 void update ()
3004 {
3005 sgVec3 dP ; sgSubVec3 ( dP, p0->getPos(), p1->getPos() ) ;
3006 sgVec3 dV ; sgSubVec3 ( dV, p0->getVel(), p1->getVel() ) ;
3007
3008 float L = sgLengthVec3 ( dP ) ; if ( L == 0.0f ) L = 0.0000001f ;
3009 float H = ( L - restLength ) * stiffness ;
3010 float D = sgScalarProductVec3 ( dV, dP ) * damping / L ;
3011
3012 sgVec3 F ; sgScaleVec3 ( F, dP, - ( H + D ) / L ) ;
3013
3014 p0 -> addForce ( F ) ;
3015 p1 -> subForce ( F ) ;
3016 }
3017
3018 } ;
3019
3020
3021 /*
3022 It must be true that (x % NOISE_WRAP_INDEX) == (x & NOISE_MOD_MASK)
3023 so NOISE_WRAP_INDEX must be a power of two, and NOISE_MOD_MASK must be
3024 that power of 2 - 1. as indices are implemented, as unsigned chars,
3025 NOISE_WRAP_INDEX shoud be less than or equal to 256.
3026 There's no good reason to change it from 256, really.
3027
3028 NOISE_LARGE_PWR2 is a large power of 2, we'll go for 4096, to add to
3029 negative numbers in order to make them positive
3030 */
3031
3032 #define SG_PERLIN_NOISE_WRAP_INDEX 256
3033 #define SG_PERLIN_NOISE_MOD_MASK 255
3034 #define SG_PERLIN_NOISE_LARGE_PWR2 4096
3035
3036
3037
3038 class sgPerlinNoise_1D
3039 {
3040 private:
3041
3042 SGfloat gradTable [ SG_PERLIN_NOISE_WRAP_INDEX * 2 + 2 ] ;
3043
3044 public:
3045
3046 sgPerlinNoise_1D () ;
3047
3048 void regenerate () ;
3049
3050 SGfloat getNoise ( SGfloat x ) ;
3051 } ;
3052
3053
3054
3055 class sgPerlinNoise_2D
3056 {
3057 private:
3058
3059 sgVec2 gradTable [ SG_PERLIN_NOISE_WRAP_INDEX * 2 + 2 ] ;
3060
3061 public:
3062
3063 sgPerlinNoise_2D () ;
3064
3065 void regenerate () ;
3066
3067 SGfloat getNoise ( sgVec2 pos ) ;
getNoise(SGfloat x,SGfloat y)3068 SGfloat getNoise ( SGfloat x, SGfloat y )
3069 {
3070 sgVec2 p ;
3071 sgSetVec2 ( p, x, y ) ;
3072 return getNoise ( p ) ;
3073 }
3074 } ;
3075
3076
3077
3078 class sgPerlinNoise_3D
3079 {
3080 private:
3081
3082 sgVec3 gradTable [ SG_PERLIN_NOISE_WRAP_INDEX * 2 + 2 ] ;
3083
3084 public:
3085
3086 sgPerlinNoise_3D () ;
3087
3088 void regenerate () ;
3089
3090 SGfloat getNoise ( sgVec3 pos ) ;
getNoise(SGfloat x,SGfloat y,SGfloat z)3091 SGfloat getNoise ( SGfloat x, SGfloat y, SGfloat z )
3092 {
3093 sgVec3 p ;
3094 sgSetVec3 ( p, x, y, z ) ;
3095 return getNoise ( p ) ;
3096 }
3097 } ;
3098
3099
3100
3101 #endif
3102
3103