1 /* NAME:
2 E3Math.c
3
4 DESCRIPTION:
5 Math routines for Quesa.
6
7 Note that these routines are allowed to call other E3foo routines for
8 speed, to avoid the trip back out through the Q3foo interface.
9
10 COPYRIGHT:
11 Copyright (c) 1999-2004, Quesa Developers. All rights reserved.
12
13 For the current release of Quesa, please see:
14
15 <http://www.quesa.org/>
16
17 Redistribution and use in source and binary forms, with or without
18 modification, are permitted provided that the following conditions
19 are met:
20
21 o Redistributions of source code must retain the above copyright
22 notice, this list of conditions and the following disclaimer.
23
24 o Redistributions in binary form must reproduce the above
25 copyright notice, this list of conditions and the following
26 disclaimer in the documentation and/or other materials provided
27 with the distribution.
28
29 o Neither the name of Quesa nor the names of its contributors
30 may be used to endorse or promote products derived from this
31 software without specific prior written permission.
32
33 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
34 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
35 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
36 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
37 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
38 SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
39 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
40 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
41 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
42 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
43 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
44 ___________________________________________________________________________
45 */
46 //=============================================================================
47 // Include files
48 //-----------------------------------------------------------------------------
49 #include "E3Prefix.h"
50 #include "E3Math.h"
51 #include "E3Utils.h"
52
53
54
55
56
57 //=============================================================================
58 // Internal functions
59 //-----------------------------------------------------------------------------
60 // Overview of matrix determinants and inverses
61 //-----------------------------------------------------------------------------
62 // To calculate the determinant of an NxN matrix with the explicit formula,
63 // one must calculate N! terms, each involving N factors. Thus it is
64 // necessary to perform N!*(N-1) multiplications and N!-1 additions and
65 // subtractions.
66 //
67 // To calculate the determinant of an NxN matrix using Gaussian elimination
68 // requires N*(N-1)/2 divisions, (N-1)*(2*N^2-N+6)/6 multiplications and
69 // N*(N-1)*(2*N-1)/6 subtractions.
70 //
71 // Explicit Formula Gaussian Elimination
72 // N mul/div add/sub mul/div add/sub
73 // 2 2 1 3 1
74 // 3 12 5 10 5
75 // 4 72 23 23 14
76 //
77 // We can see that, in terms of the number of floating point operations,
78 // Gaussian elimination is faster than the explicit formula, even for N as
79 // as small as 3 or 4.
80 //
81 // To calculate the inverse of an NxN matrix using the explicit formula
82 // (Cramer's rule), we must calculate 1 determinant of order N and NxN
83 // determinants of order N-1. Then we must perform N^2 divisions. In total,
84 // calculating the inverse requires N^2 divisions, N!*(N^2-N-1)
85 // multiplications and (N+1)!-N^2-1 additions and subtractions.
86 //
87 // To calculate the inverse of an NxN matrix using Gauss-Jordon
88 // elimination requires N^2 divisions, N^2*(N-1) multiplications (and
89 // N^2*(N-1) subtractions.
90 //
91 // Cramer's Rule Gauss-Jordon Elimination
92 // N mul/div add/sub mul/div add/sub
93 // 2 6 1 8 4
94 // 3 39 14 27 18
95 // 4 280 103 64 48
96 //
97 // We can see that, in terms of the number of floating point operations,
98 // Gauss-Jordon elimination is faster than Cramer's rule, even for N as
99 // as small as 3 or 4.
100 //
101 // More importantly, the explicit formulas for calculating the determinant
102 // and inverse of a matrix are unstable: Round off errors are not always
103 // negligible.
104 //
105 // In conclusion, Gaussian elimination and Gauss-Jordon elimination are
106 // preferable to the explicit formulas for finding the determinant and
107 // inverse, respectively, of an NxN matrix, even for N as small as 3 or 4.
108 //
109 //
110 // Originally, Quesa used e3matrix_determinant and e3matrix_invert to
111 // calculate determinants and inverses. These have now been replaced with
112 // specific 3x3 and 4x4 implementations, but the original implementations
113 // are preserved in case we need a generic NxN approach in the future.
114 //-----------------------------------------------------------------------------
115
116
117
118
119
120 //=============================================================================
121 // e3matrix3x3_determinant : Returns the determinant of the given 3x3 matrix.
122 //-----------------------------------------------------------------------------
123 // Note : The algorithm modifies the input matrix a.
124 //
125 // This function uses Gaussian elimination with full pivoting to
126 // reduce the matrix to upper triangular form. Then the determinant
127 // is merely (plus or minus) the product of the diagonal elements.
128 //
129 // Although the reduction would create 1's along the diagonal and
130 // 0's below the diagonal, these elements are not calculated because
131 // they are not needed.
132 //
133 // See Press, et al., "Numerical Recipes in C", 2nd ed., pp. 32 ff.
134 //-----------------------------------------------------------------------------
135 static float
e3matrix3x3_determinant(TQ3Matrix3x3 * a)136 e3matrix3x3_determinant(TQ3Matrix3x3* a)
137 {
138 #define A(x,y) a->value[x][y]
139
140 TQ3Int32 iSign, iPivot, jPivot;
141 TQ3Int32 i, j, k;
142 float determinant, big, element;
143
144 // iSign is +1 or -1, depending on the number of row or column exchanges
145 iSign = 1;
146
147 // Loop over 3 pivots
148 for (k = 0; k < 3; ++k)
149 {
150 // Search unpivoted part of matrix for largest element to pivot on
151 big = -1.0f;
152 for (i = k; i < 3; ++i)
153 {
154 for (j = k; j < 3; ++j)
155 {
156 // Calculate absolute value of current element
157 element = A(i,j);
158 if (element < 0.0f)
159 element = -element;
160
161 // Compare current element to largest element so far
162 if (element > big)
163 {
164 big = element;
165 iPivot = i;
166 jPivot = j;
167 }
168 }
169 }
170
171 // If largest element is 0, the matrix is singular
172 // (It could still be -1 if the matrix contained "nan" values.)
173 if (big <= 0.0f)
174 return(0.0f);
175
176 // If necessary, put pivot element on diagonal at (k,k)
177 if (iPivot != k)
178 {
179 // Exchange rows
180 iSign = -iSign;
181 for (j = k; j < 3; ++j)
182 E3Float_Swap(A(iPivot,j), A(k,j));
183 }
184 if (jPivot != k)
185 {
186 // Exchange columns
187 iSign = -iSign;
188 for (i = k; i < 3; ++i)
189 E3Float_Swap(A(i,jPivot), A(i,k));
190 }
191
192 // Divide pivot row (to the right of the pivot column) by pivot element
193 //
194 // Note: If we were dividing by the same element many times, it would
195 // make sense to multiply by its inverse. Since we divide by a given
196 // element at most 2 (3) times for a 3x3 (4x4) matrix -- and often
197 // less -- it doesn't make sense to pay for the extra floating-point
198 // operation.
199 element = A(k,k);
200 for (j = k+1; j < 3; ++j)
201 A(k,j) /= element;
202
203 // Reduce rows below pivot row (and to the right of the pivot column)
204 for (i = k+1; i < 3; ++i)
205 {
206 element = A(i,k);
207 for (j = k+1; j < 3; ++j)
208 A(i,j) -= A(k,j)*element;
209 }
210 }
211
212 // Now that the matrix is upper triangular, calculate the determinant as
213 // the product of the diagonal elements
214 determinant = A(0,0);
215 for (k = 1; k < 3; ++k)
216 determinant *= A(k,k);
217 if (iSign < 0)
218 determinant = -determinant;
219
220 return(determinant);
221
222 #undef A
223 }
224
225
226
227
228
229 //=============================================================================
230 // e3matrix3x3_invert : Transforms the given 3x3 matrix into its inverse.
231 //-----------------------------------------------------------------------------
232 // Note : This function uses Gauss-Jordon elimination with full pivoting
233 // to transform the given matrix to the identity matrix while
234 // transforming the identity matrix to the inverse. As the given
235 // matrix is reduced to 1's and 0's column-by-column, the inverse
236 // matrix is created in its place column-by-column.
237 //
238 // See Press, et al., "Numerical Recipes in C", 2nd ed., pp. 32 ff.
239 //-----------------------------------------------------------------------------
240 static void
e3matrix3x3_invert(TQ3Matrix3x3 * a)241 e3matrix3x3_invert(TQ3Matrix3x3* a)
242 {
243 #define A(x,y) a->value[x][y]
244
245 TQ3Int32 irow, icol;
246 TQ3Int32 i, j, k; // *** WARNING: 'k' must be a SIGNED integer ***
247 float big, element;
248 TQ3Int32 ipiv[3], indxr[3], indxc[3];
249
250 // Initialize ipiv: ipiv[j] is 0 (1) if row/column j has not (has) been pivoted
251 for (j = 0; j < 3; ++j)
252 ipiv[j] = 0;
253
254 // Loop over 3 pivots
255 for (k = 0; k < 3; ++k)
256 {
257 // Search unpivoted part of matrix for largest element to pivot on
258 big = -1.0f;
259 for (i = 0; i < 3; ++i)
260 {
261 if (ipiv[i])
262 continue;
263
264 for (j = 0; j < 3; ++j)
265 {
266 if (ipiv[j])
267 continue;
268
269 // Calculate absolute value of current element
270 element = A(i,j);
271 if (element < 0.0f)
272 element = -element;
273
274 // Compare current element to largest element so far
275 if (element > big)
276 {
277 big = element;
278 irow = i;
279 icol = j;
280 }
281 }
282 }
283
284 // If largest element is 0, the matrix is singular
285 // (If there are "nan" values in the matrix, "big" may still be -1.0.)
286 if (big <= 0.0f)
287 {
288 E3ErrorManager_PostError(kQ3ErrorNonInvertibleMatrix, kQ3False);
289 return;
290 }
291
292 // Mark pivot row and column
293 ++ipiv[icol];
294 indxr[k] = irow;
295 indxc[k] = icol;
296
297 // If necessary, exchange rows to put pivot element on diagonal
298 if (irow != icol)
299 {
300 for (j = 0; j < 3; ++j)
301 E3Float_Swap(A(irow,j), A(icol,j));
302 }
303
304 // Divide pivot row by pivot element
305 //
306 // Note: If we were dividing by the same element many times, it would
307 // make sense to multiply by its inverse. Since we divide by a given
308 // elemen only 3 (4) times for a 3x3 (4x4) matrix, it doesn't make sense
309 // to pay for the extra floating-point operation.
310 element = A(icol,icol);
311 A(icol,icol) = 1.0f; // overwrite original matrix with inverse
312 for (j = 0; j < 3; ++j)
313 A(icol,j) /= element;
314
315 // Reduce other rows
316 for (i = 0; i < 3; ++i)
317 {
318 if (i == icol)
319 continue;
320
321 element = A(i,icol);
322 A(i,icol) = 0.0f; // overwrite original matrix with inverse
323 for (j = 0; j < 3; ++j)
324 A(i,j) -= A(icol,j)*element;
325 }
326 }
327
328 // Permute columns
329 for (k = 3; --k >= 0; ) // *** WARNING: 'k' must be a SIGNED integer ***
330 {
331 if (indxr[k] != indxc[k])
332 {
333 for (i = 0; i < 3; ++i)
334 E3Float_Swap(A(i,indxr[k]), A(i,indxc[k]));
335 }
336 }
337
338 #undef A
339 }
340
341
342
343
344
345 //=============================================================================
346 // e3matrix4x4_determinant : Returns the determinant of the given 4x4 matrix.
347 //-----------------------------------------------------------------------------
348 // Note : This function uses Gaussian elimination with full pivoting to
349 // reduce the matrix to upper triangular form. Then the determinant
350 // is merely (plus or minus) the product of the diagonal elements.
351 //
352 // Although the reduction would create 1's along the diagonal and
353 // 0's below the diagonal, these elements are not calculated because
354 // they are not needed.
355 //
356 // See Press, et al., "Numerical Recipes in C", 2nd ed., pp. 32 ff.
357 //-----------------------------------------------------------------------------
358 static float
e3matrix4x4_determinant(TQ3Matrix4x4 * a)359 e3matrix4x4_determinant(TQ3Matrix4x4* a)
360 {
361 #define A(x,y) a->value[x][y]
362
363 TQ3Int32 iSign, iPivot, jPivot;
364 TQ3Int32 i, j, k;
365 float determinant, big, element;
366
367 // iSign is +1 or -1, depending on the number of row or column exchanges
368 iSign = 1;
369
370 // Loop over 4 pivots
371 for (k = 0; k < 4; ++k)
372 {
373 // Search unpivoted part of matrix for largest element to pivot on
374 big = -1.0f;
375 for (i = k; i < 4; ++i)
376 {
377 for (j = k; j < 4; ++j)
378 {
379 // Calculate absolute value of current element
380 element = A(i,j);
381 if (element < 0.0f)
382 element = -element;
383
384 // Compare current element to largest element so far
385 if (element > big)
386 {
387 big = element;
388 iPivot = i;
389 jPivot = j;
390 }
391 }
392 }
393
394 // If largest element is 0, the matrix is singular
395 // (It could still be -1 if the matrix contained "nan" values.)
396 if (big <= 0.0f)
397 return(0.0f);
398
399 // If necessary, put pivot element on diagonal at (k,k)
400 if (iPivot != k)
401 {
402 // Exchange rows
403 iSign = -iSign;
404 for (j = k; j < 4; ++j)
405 E3Float_Swap(A(iPivot,j), A(k,j));
406 }
407 if (jPivot != k)
408 {
409 // Exchange columns
410 iSign = -iSign;
411 for (i = k; i < 4; ++i)
412 E3Float_Swap(A(i,jPivot), A(i,k));
413 }
414
415 // Divide pivot row (to the right of the pivot column) by pivot element
416 //
417 // Note: If we were dividing by the same element many times, it would
418 // make sense to multiply by its inverse. Since we divide by a given
419 // element at most 2 (3) times for a 3x3 (4x4) matrix -- and often
420 // less -- it doesn't make sense to pay for the extra floating-point
421 // operation.
422 element = A(k,k);
423 for (j = k+1; j < 4; ++j)
424 A(k,j) /= element;
425
426 // Reduce rows below pivot row (and to the right of the pivot column)
427 for (i = k+1; i < 4; ++i)
428 {
429 element = A(i,k);
430 for (j = k+1; j < 4; ++j)
431 A(i,j) -= A(k,j)*element;
432 }
433 }
434
435 // Now that the matrix is upper triangular, calculate the determinant as
436 // the product of the diagonal elements
437 determinant = A(0,0);
438 for (k = 1; k < 4; ++k)
439 determinant *= A(k,k);
440 if (iSign < 0)
441 determinant = -determinant;
442
443 return(determinant);
444
445 #undef A
446 }
447
448
449
450
451
452 //=============================================================================
453 // e3matrix4x4_invert : Transforms the given 4x4 matrix into its inverse.
454 //-----------------------------------------------------------------------------
455 // Note : This function uses Gauss-Jordon elimination with full pivoting
456 // to transform the given matrix to the identity matrix while
457 // transforming the identity matrix to the inverse. As the given
458 // matrix is reduced to 1's and 0's column-by-column, the inverse
459 // matrix is created in its place column-by-column.
460 //
461 // See Press, et al., "Numerical Recipes in C", 2nd ed., pp. 32 ff.
462 //-----------------------------------------------------------------------------
463 static void
e3matrix4x4_invert(TQ3Matrix4x4 * a)464 e3matrix4x4_invert(TQ3Matrix4x4* a)
465 {
466 #define A(x,y) a->value[x][y]
467
468 TQ3Int32 irow, icol;
469 TQ3Int32 i, j, k; // *** WARNING: 'k' must be a SIGNED integer ***
470 float big, element;
471 TQ3Int32 ipiv[4], indxr[4], indxc[4];
472
473 // Initialize ipiv: ipiv[j] is 0 (1) if row/column j has not (has) been pivoted
474 for (j = 0; j < 4; ++j)
475 ipiv[j] = 0;
476
477 // Loop over 4 pivots
478 for (k = 0; k < 4; ++k)
479 {
480 // Search unpivoted part of matrix for largest element to pivot on
481 big = -1.0f;
482 for (i = 0; i < 4; ++i)
483 {
484 if (ipiv[i])
485 continue;
486
487 for (j = 0; j < 4; ++j)
488 {
489 if (ipiv[j])
490 continue;
491
492 // Calculate absolute value of current element
493 element = A(i,j);
494 if (element < 0.0f)
495 element = -element;
496
497 // Compare current element to largest element so far
498 if (element > big)
499 {
500 big = element;
501 irow = i;
502 icol = j;
503 }
504 }
505 }
506
507 // If largest element is 0, the matrix is singular
508 // (If there are "nan" values in the matrix, "big" may still be -1.0.)
509 if (big <= 0.0f)
510 {
511 E3ErrorManager_PostError(kQ3ErrorNonInvertibleMatrix, kQ3False);
512 return;
513 }
514
515 // Mark pivot row and column
516 ++ipiv[icol];
517 indxr[k] = irow;
518 indxc[k] = icol;
519
520 // If necessary, exchange rows to put pivot element on diagonal
521 if (irow != icol)
522 {
523 for (j = 0; j < 4; ++j)
524 E3Float_Swap(A(irow,j), A(icol,j));
525 }
526
527 // Divide pivot row by pivot element
528 //
529 // Note: If we were dividing by the same element many times, it would
530 // make sense to multiply by its inverse. Since we divide by a given
531 // elemen only 3 (4) times for a 3x3 (4x4) matrix, it doesn't make sense
532 // to pay for the extra floating-point operation.
533 element = A(icol,icol);
534 A(icol,icol) = 1.0f; // overwrite original matrix with inverse
535 for (j = 0; j < 4; ++j)
536 A(icol,j) /= element;
537
538 // Reduce other rows
539 for (i = 0; i < 4; ++i)
540 {
541 if (i == icol)
542 continue;
543
544 element = A(i,icol);
545 A(i,icol) = 0.0f; // overwrite original matrix with inverse
546 for (j = 0; j < 4; ++j)
547 A(i,j) -= A(icol,j)*element;
548 }
549 }
550
551 // Permute columns
552 for (k = 4; --k >= 0; ) // *** WARNING: 'k' must be a SIGNED integer ***
553 {
554 if (indxr[k] != indxc[k])
555 {
556 for (i = 0; i < 4; ++i)
557 E3Float_Swap(A(i,indxr[k]), A(i,indxc[k]));
558 }
559 }
560
561 #undef A
562 }
563
564
565
566
567
568 //=============================================================================
569 // e3vector3d_below_tolerance : Is a vector below a threshold?
570 //-----------------------------------------------------------------------------
571 static TQ3Boolean
e3vector3d_below_tolerance(const TQ3Vector3D * theVector,float tol)572 e3vector3d_below_tolerance(const TQ3Vector3D *theVector, float tol)
573 { float vecLen2;
574 float tol2;
575
576
577
578 // Check to see if we're below tolerance
579 vecLen2 = E3Vector3D_LengthSquared(theVector);
580 tol2 = tol * tol;
581
582 return (TQ3Boolean) (vecLen2 < tol2);
583 }
584
585
586
587
588
589 //=============================================================================
590 // e3bounding_box_accumulate_point3D : Accumulate (union) a point into a
591 // nonempty bounding box.
592 //-----------------------------------------------------------------------------
593 #pragma mark -
594 static void
e3bounding_box_accumulate_point3D(TQ3BoundingBox * bBox,const TQ3Point3D * point3D)595 e3bounding_box_accumulate_point3D(TQ3BoundingBox *bBox, const TQ3Point3D *point3D)
596 {
597 float x = point3D->x;
598 float y = point3D->y;
599 float z = point3D->z;
600
601 if (x < bBox->min.x)
602 bBox->min.x = x;
603 else if (x > bBox->max.x)
604 bBox->max.x = x;
605
606 if (y < bBox->min.y)
607 bBox->min.y = y;
608 else if (y > bBox->max.y)
609 bBox->max.y = y;
610
611 if (z < bBox->min.z)
612 bBox->min.z = z;
613 else if (z > bBox->max.z)
614 bBox->max.z = z;
615 }
616
617
618
619
620
621 //=============================================================================
622 // Public functions
623 //-----------------------------------------------------------------------------
624 // E3Vector2D_Set : Set 2D vector.
625 //-----------------------------------------------------------------------------
626 #pragma mark -
627 TQ3Vector2D *
E3Vector2D_Set(TQ3Vector2D * vector2D,float x,float y)628 E3Vector2D_Set(TQ3Vector2D *vector2D, float x, float y)
629 {
630 Q3FastVector2D_Set(vector2D, x, y);
631 return(vector2D);
632 }
633
634
635
636
637
638 //=============================================================================
639 // E3Vector3D_Set : Set 3D vector.
640 //-----------------------------------------------------------------------------
641 TQ3Vector3D *
E3Vector3D_Set(TQ3Vector3D * vector3D,float x,float y,float z)642 E3Vector3D_Set(TQ3Vector3D *vector3D, float x, float y, float z)
643 {
644 Q3FastVector3D_Set(vector3D, x, y, z);
645 return(vector3D);
646 }
647
648
649
650
651
652 //=============================================================================
653 // E3Point2D_Set : Set 2D point.
654 //-----------------------------------------------------------------------------
655 TQ3Point2D *
E3Point2D_Set(TQ3Point2D * point2D,float x,float y)656 E3Point2D_Set(TQ3Point2D *point2D, float x, float y)
657 {
658 Q3FastPoint2D_Set(point2D, x, y);
659 return(point2D);
660 }
661
662
663
664
665
666 //=============================================================================
667 // E3Param2D_Set : Set 2D parametric point.
668 //-----------------------------------------------------------------------------
669 TQ3Param2D *
E3Param2D_Set(TQ3Param2D * param2D,float u,float v)670 E3Param2D_Set(TQ3Param2D *param2D, float u, float v)
671 {
672 Q3FastParam2D_Set(param2D, u, v);
673 return(param2D);
674 }
675
676
677
678
679
680 //=============================================================================
681 // E3RationalPoint3D_Set : Set RationalPoint3D.
682 //-----------------------------------------------------------------------------
683 // Note : This is an extended 2D point used with 2D transformations.
684 //-----------------------------------------------------------------------------
685 TQ3RationalPoint3D *
E3RationalPoint3D_Set(TQ3RationalPoint3D * rationalPoint3D,float x,float y,float w)686 E3RationalPoint3D_Set(TQ3RationalPoint3D *rationalPoint3D, float x, float y, float w)
687 {
688 Q3FastRationalPoint3D_Set(rationalPoint3D, x, y, w);
689 return(rationalPoint3D);
690 }
691
692
693
694
695
696 //=============================================================================
697 // E3Point3D_Set : Set 3D point.
698 //-----------------------------------------------------------------------------
699 TQ3Point3D *
E3Point3D_Set(TQ3Point3D * point3D,float x,float y,float z)700 E3Point3D_Set(TQ3Point3D *point3D, float x, float y, float z)
701 {
702 Q3FastPoint3D_Set(point3D, x, y, z);
703 return(point3D);
704 }
705
706
707
708
709
710 //=============================================================================
711 // E3RationalPoint4D_Set : Set RationalPoint4D.
712 //-----------------------------------------------------------------------------
713 // Note : This is an extended 3D point used with 3D transformations.
714 //-----------------------------------------------------------------------------
715 TQ3RationalPoint4D *
E3RationalPoint4D_Set(TQ3RationalPoint4D * rationalPoint4D,float x,float y,float z,float w)716 E3RationalPoint4D_Set(TQ3RationalPoint4D *rationalPoint4D, float x, float y, float z, float w)
717 {
718 Q3FastRationalPoint4D_Set(rationalPoint4D, x, y, z, w);
719 return(rationalPoint4D);
720 }
721
722
723
724
725
726 //=============================================================================
727 // E3PolarPoint_Set : Set polar point.
728 //-----------------------------------------------------------------------------
729 TQ3PolarPoint *
E3PolarPoint_Set(TQ3PolarPoint * polarPoint,float r,float theta)730 E3PolarPoint_Set(TQ3PolarPoint *polarPoint, float r, float theta)
731 {
732 Q3FastPolarPoint_Set(polarPoint, r, theta);
733 return(polarPoint);
734 }
735
736
737
738
739
740 //=============================================================================
741 // E3SphericalPoint_Set : Set spherical point.
742 //-----------------------------------------------------------------------------
743 TQ3SphericalPoint *
E3SphericalPoint_Set(TQ3SphericalPoint * sphericalPoint,float rho,float theta,float phi)744 E3SphericalPoint_Set(TQ3SphericalPoint *sphericalPoint, float rho, float theta, float phi)
745 {
746 Q3FastSphericalPoint_Set(sphericalPoint, rho, theta, phi);
747 return(sphericalPoint);
748 }
749
750
751
752
753
754 //=============================================================================
755 // E3Vector2D_To3D : Convert 2D vector to 3D, setting z to 1.
756 //-----------------------------------------------------------------------------
757 // Note : This operation makes no sense mathematically.
758 //-----------------------------------------------------------------------------
759 #pragma mark -
760 TQ3Vector3D *
E3Vector2D_To3D(const TQ3Vector2D * vector2D,TQ3Vector3D * result)761 E3Vector2D_To3D(const TQ3Vector2D *vector2D, TQ3Vector3D *result)
762 {
763 Q3FastVector2D_To3D(vector2D, result);
764 return(result);
765 }
766
767
768
769
770
771 //=============================================================================
772 // E3Vector2D_ToRationalPoint3D : Convert 2D vector to 3D rational point,
773 // setting w to 0.
774 //-----------------------------------------------------------------------------
775 TQ3RationalPoint3D *
E3Vector2D_ToRationalPoint3D(const TQ3Vector2D * vector2D,TQ3RationalPoint3D * result)776 E3Vector2D_ToRationalPoint3D(const TQ3Vector2D *vector2D, TQ3RationalPoint3D *result)
777 {
778 Q3FastVector2D_ToRationalPoint3D(vector2D, result);
779 return(result);
780 }
781
782
783
784
785
786 //=============================================================================
787 // E3Vector3D_To2D : Convert 3D vector to 2D, dividing by z.
788 //-----------------------------------------------------------------------------
789 // Note : This operation makes no sense mathematically.
790 //-----------------------------------------------------------------------------
791 TQ3Vector2D *
E3Vector3D_To2D(const TQ3Vector3D * vector3D,TQ3Vector2D * result)792 E3Vector3D_To2D(const TQ3Vector3D *vector3D, TQ3Vector2D *result)
793 {
794 Q3FastVector3D_To2D(vector3D, result);
795 return(result);
796 }
797
798
799
800
801
802 //=============================================================================
803 // E3RationalPoint3D_ToVector2D : Convert 3D rational point to 2D vector,
804 // discarding w.
805 //-----------------------------------------------------------------------------
806 TQ3Vector2D *
E3RationalPoint3D_ToVector2D(const TQ3RationalPoint3D * rationalPoint3D,TQ3Vector2D * result)807 E3RationalPoint3D_ToVector2D(const TQ3RationalPoint3D *rationalPoint3D, TQ3Vector2D *result)
808 {
809 Q3FastRationalPoint3D_ToVector2D(rationalPoint3D, result);
810 return(result);
811 }
812
813
814
815
816
817 //=============================================================================
818 // E3Vector3D_ToRationalPoint4D : Convert 3D vector to 4D rational point,
819 // setting w to 0.
820 //-----------------------------------------------------------------------------
821 TQ3RationalPoint4D *
E3Vector3D_ToRationalPoint4D(const TQ3Vector3D * vector3D,TQ3RationalPoint4D * result)822 E3Vector3D_ToRationalPoint4D(const TQ3Vector3D *vector3D, TQ3RationalPoint4D *result)
823 {
824 Q3FastVector3D_ToRationalPoint4D(vector3D, result);
825 return(result);
826 }
827
828
829
830
831
832 //=============================================================================
833 // E3RationalPoint4D_ToVector3D : Convert 4D rational point to 3D vector,
834 // discarding w.
835 //-----------------------------------------------------------------------------
836 TQ3Vector3D *
E3RationalPoint4D_ToVector3D(const TQ3RationalPoint4D * rationalPoint4D,TQ3Vector3D * result)837 E3RationalPoint4D_ToVector3D(const TQ3RationalPoint4D *rationalPoint4D, TQ3Vector3D *result)
838 {
839 Q3FastRationalPoint4D_ToVector3D(rationalPoint4D, result);
840 return(result);
841 }
842
843
844
845
846
847 //=============================================================================
848 // E3Point2D_To3D : Convert 2D point to rational 3D, setting w to 1.
849 //-----------------------------------------------------------------------------
850 // Note : The QD3D version incorrectly declares the type of 'result' to
851 // be TQ3Point3D rather than TQ3RationalPoint3D.
852 //
853 // At a binary level there is no difference, but at a source code
854 // level the QD3D version forces the use of the incorrect type or
855 // type casting.
856 //
857 // Since the QD3D declaration is incorrect, we have corrected this
858 // for Quesa - even though it may require a recast in your code.
859 //-----------------------------------------------------------------------------
860 TQ3RationalPoint3D *
E3Point2D_To3D(const TQ3Point2D * point2D,TQ3RationalPoint3D * result)861 E3Point2D_To3D(const TQ3Point2D *point2D, TQ3RationalPoint3D *result)
862 {
863 Q3FastPoint2D_To3D(point2D, result);
864 return(result);
865 }
866
867
868
869
870
871 //=============================================================================
872 // E3RationalPoint3D_To2D : Convert rational 3D point to 2D, dividing by w.
873 //-----------------------------------------------------------------------------
874 TQ3Point2D *
E3RationalPoint3D_To2D(const TQ3RationalPoint3D * rationalPoint3D,TQ3Point2D * result)875 E3RationalPoint3D_To2D(const TQ3RationalPoint3D *rationalPoint3D, TQ3Point2D *result)
876 {
877 Q3FastRationalPoint3D_To2D(rationalPoint3D, result);
878 return(result);
879 }
880
881
882
883
884
885 //============================================================================
886 // E3Point3D_To4D : Convert 3D point to rational 4D, setting w to 1.
887 //-----------------------------------------------------------------------------
888 TQ3RationalPoint4D *
E3Point3D_To4D(const TQ3Point3D * point3D,TQ3RationalPoint4D * result)889 E3Point3D_To4D(const TQ3Point3D *point3D, TQ3RationalPoint4D *result)
890 {
891 Q3FastPoint3D_To4D(point3D, result);
892 return(result);
893 }
894
895
896
897
898
899 //=============================================================================
900 // E3RationalPoint4D_To3D : Convert rational 4D point to 3D, dividing by w.
901 //-----------------------------------------------------------------------------
902 TQ3Point3D *
E3RationalPoint4D_To3D(const TQ3RationalPoint4D * rationalPoint4D,TQ3Point3D * result)903 E3RationalPoint4D_To3D(const TQ3RationalPoint4D *rationalPoint4D, TQ3Point3D *result)
904 {
905 Q3FastRationalPoint4D_To3D(rationalPoint4D, result);
906 return(result);
907 }
908
909
910
911
912
913 //=============================================================================
914 // E3Point2D_ToPolar : Convert 2D cartesian point to polar coordinates.
915 //-----------------------------------------------------------------------------
916 // Note : The angle (theta) here is measured counter-clockwise
917 // from the +x axis.
918 //-----------------------------------------------------------------------------
919 #pragma mark -
920 TQ3PolarPoint *
E3Point2D_ToPolar(const TQ3Point2D * point2D,TQ3PolarPoint * result)921 E3Point2D_ToPolar(const TQ3Point2D *point2D, TQ3PolarPoint *result)
922 {
923 float x = point2D->x;
924 float y = point2D->y;
925
926 if (x == 0.0f && y == 0.0f)
927 {
928 result->r = result->theta = 0.0f;
929 }
930 else
931 {
932 result->r = E3Math_SquareRoot(x*x + y*y);
933 result->theta = (float) atan2(y, x);
934 if (result->theta < 0.0f)
935 result->theta += kQ32Pi;
936 }
937
938 return(result);
939 }
940
941
942
943
944
945 //=============================================================================
946 // E3PolarPoint_ToPoint2D : Convert 2D polar point to cartesian
947 // coordinates.
948 //-----------------------------------------------------------------------------
949 // Note : The angle (theta) here is measured counter-clockwise
950 // from the +x axis.
951 //-----------------------------------------------------------------------------
952 TQ3Point2D *
E3PolarPoint_ToPoint2D(const TQ3PolarPoint * polarPoint,TQ3Point2D * result)953 E3PolarPoint_ToPoint2D(const TQ3PolarPoint *polarPoint, TQ3Point2D *result)
954 {
955 Q3FastPolarPoint_ToPoint2D(polarPoint, result);
956 return(result);
957 }
958
959
960
961
962
963 //=============================================================================
964 // E3Point3D_ToSpherical : Convert 3D cartesian point to spherical
965 // coordinates.
966 //-----------------------------------------------------------------------------
967 // Note : A pretty good reference for 3D coordinate conversions is:
968 //
969 // http://www.geom.umn.edu/docs/reference/CRC-formulas/node42.html
970 //-----------------------------------------------------------------------------
971 TQ3SphericalPoint *
E3Point3D_ToSpherical(const TQ3Point3D * point3D,TQ3SphericalPoint * result)972 E3Point3D_ToSpherical(const TQ3Point3D *point3D, TQ3SphericalPoint *result)
973 {
974 float x = point3D->x;
975 float y = point3D->y;
976 float z = point3D->z;
977
978 if (x == 0.0f && y == 0.0f)
979 {
980 if (z >= 0.0f)
981 {
982 result->rho = z;
983 result->phi = 0.0f;
984 }
985 else
986 {
987 result->rho = -z;
988 result->phi = kQ3PiOver2;
989 }
990 result->theta = 0.0f;
991 }
992 else
993 {
994 result->rho = E3Math_SquareRoot(x*x + y*y + z*z);
995 result->phi = (float) acos(z/result->rho);
996 result->theta = (float) atan2(y, x);
997 if (result->theta < 0.0f)
998 result->theta += kQ32Pi;
999 }
1000
1001 return(result);
1002 }
1003
1004
1005
1006
1007
1008 //=============================================================================
1009 // E3SphericalPoint_ToPoint3D : Convert 3D spherical point to cartesian
1010 // coordinates.
1011 //-----------------------------------------------------------------------------
1012 TQ3Point3D *
E3SphericalPoint_ToPoint3D(const TQ3SphericalPoint * sphericalPoint,TQ3Point3D * result)1013 E3SphericalPoint_ToPoint3D(const TQ3SphericalPoint *sphericalPoint,
1014 TQ3Point3D *result)
1015 {
1016 float rhoSinPhi = sphericalPoint->rho * ((float) sin(sphericalPoint->phi));
1017 result->x = rhoSinPhi * ((float) cos(sphericalPoint->theta));
1018 result->y = rhoSinPhi * ((float) sin(sphericalPoint->theta));
1019 result->z = sphericalPoint->rho * ((float) cos(sphericalPoint->phi));
1020 return(result);
1021 }
1022
1023
1024
1025
1026
1027 //=============================================================================
1028 // E3Vector2D_Dot : Return dot product of v1 and v2.
1029 //-----------------------------------------------------------------------------
1030 #pragma mark -
1031 float
E3Vector2D_Dot(const TQ3Vector2D * v1,const TQ3Vector2D * v2)1032 E3Vector2D_Dot(const TQ3Vector2D *v1, const TQ3Vector2D *v2)
1033 {
1034 return(Q3FastVector2D_Dot(v1, v2));
1035 }
1036
1037
1038
1039
1040
1041 //=============================================================================
1042 // E3Vector3D_Dot : Return dot product of v1 and v2.
1043 //-----------------------------------------------------------------------------
1044 float
E3Vector3D_Dot(const TQ3Vector3D * v1,const TQ3Vector3D * v2)1045 E3Vector3D_Dot(const TQ3Vector3D *v1, const TQ3Vector3D *v2)
1046 {
1047 return(Q3FastVector3D_Dot(v1, v2));
1048 }
1049
1050
1051
1052
1053
1054 //=============================================================================
1055 // E3Vector3D_DotArray : Dot pair of arrays of 3D vectors.
1056 //-----------------------------------------------------------------------------
1057 TQ3Status
E3Vector3D_DotArray(const TQ3Vector3D * inFirstVectors3D,const TQ3Vector3D * inSecondVectors3D,float * outDotProducts,TQ3Boolean * outDotLessThanZeros,TQ3Uns32 numVectors,TQ3Uns32 inStructSize,TQ3Uns32 outDotProductStructSize,TQ3Uns32 outDotLessThanZeroStructSize)1058 E3Vector3D_DotArray(
1059 const TQ3Vector3D *inFirstVectors3D,
1060 const TQ3Vector3D *inSecondVectors3D,
1061 float *outDotProducts,
1062 TQ3Boolean *outDotLessThanZeros,
1063 TQ3Uns32 numVectors,
1064 TQ3Uns32 inStructSize,
1065 TQ3Uns32 outDotProductStructSize,
1066 TQ3Uns32 outDotLessThanZeroStructSize)
1067 {
1068 float dotProduct;
1069 TQ3Uns32 i;
1070
1071 // Calculate the dot products
1072 if (outDotProducts != NULL && outDotLessThanZeros != NULL)
1073 {
1074 for (i = 0; i < numVectors; ++i)
1075 {
1076 dotProduct = Q3FastVector3D_Dot(inFirstVectors3D, inSecondVectors3D);
1077 *outDotProducts = dotProduct;
1078 *outDotLessThanZeros = (TQ3Boolean) (dotProduct < 0.0f);
1079
1080 ((const char*&) inFirstVectors3D) += inStructSize;
1081 ((const char*&) inSecondVectors3D) += inStructSize;
1082 ((char*&) outDotProducts) += outDotProductStructSize;
1083 ((char*&) outDotLessThanZeros) += outDotLessThanZeroStructSize;
1084 }
1085 }
1086
1087 else if (outDotProducts != NULL)
1088 {
1089 for (i = 0; i < numVectors; ++i)
1090 {
1091 dotProduct = Q3Vector3D_Dot(inFirstVectors3D, inSecondVectors3D);
1092 *outDotProducts = dotProduct;
1093
1094 ((const char*&) inFirstVectors3D) += inStructSize;
1095 ((const char*&) inSecondVectors3D) += inStructSize;
1096 ((char*&) outDotProducts) += outDotProductStructSize;
1097 }
1098 }
1099
1100 else
1101 {
1102 for (i = 0; i < numVectors; ++i)
1103 {
1104 dotProduct = Q3Vector3D_Dot(inFirstVectors3D, inSecondVectors3D);
1105 *outDotLessThanZeros = (TQ3Boolean) (dotProduct < 0.0f);
1106
1107 ((const char*&) inFirstVectors3D) += inStructSize;
1108 ((const char*&) inSecondVectors3D) += inStructSize;
1109 ((char*&) outDotLessThanZeros) += outDotLessThanZeroStructSize;
1110 }
1111 }
1112
1113 return(kQ3Success);
1114 }
1115
1116
1117
1118
1119
1120 //=============================================================================
1121 // E3Vector2D_Cross : Return 2D cross product of v1 and v2.
1122 //-----------------------------------------------------------------------------
1123 // Note : We assume the 2D vectors are really 3D vectors with z=0,
1124 // then return the z coordinate of the cross product (0,0,z).
1125 //-----------------------------------------------------------------------------
1126 #pragma mark -
1127 float
E3Vector2D_Cross(const TQ3Vector2D * v1,const TQ3Vector2D * v2)1128 E3Vector2D_Cross(const TQ3Vector2D *v1, const TQ3Vector2D *v2)
1129 {
1130 return(Q3FastVector2D_Cross(v1, v2));
1131 }
1132
1133
1134
1135
1136
1137 //=============================================================================
1138 // E3Point2D_CrossProductTri : Return cross product of triangle, that is
1139 // of the vectors p2-p1 and p3-p2.
1140 //-----------------------------------------------------------------------------
1141 float
E3Point2D_CrossProductTri(const TQ3Point2D * p1,const TQ3Point2D * p2,const TQ3Point2D * p3)1142 E3Point2D_CrossProductTri(const TQ3Point2D *p1, const TQ3Point2D *p2, const TQ3Point2D *p3)
1143 {
1144 return(Q3FastPoint2D_CrossProductTri(p1, p2, p3));
1145 }
1146
1147
1148
1149
1150
1151 //=============================================================================
1152 // E3Vector3D_Cross : Return 3D cross product of v1 and v2.
1153 //-----------------------------------------------------------------------------
1154 // Note : 'result' may be the same as 'v1' and/or 'v2'.
1155 //-----------------------------------------------------------------------------
1156 TQ3Vector3D *
E3Vector3D_Cross(const TQ3Vector3D * v1,const TQ3Vector3D * v2,TQ3Vector3D * result)1157 E3Vector3D_Cross(const TQ3Vector3D *v1, const TQ3Vector3D *v2, TQ3Vector3D *result)
1158 {
1159 Q3FastVector3D_Cross(v1, v2, result);
1160 return(result);
1161 }
1162
1163
1164
1165
1166
1167 //=============================================================================
1168 // E3Point3D_CrossProductTri : Return cross product of triangle, that is
1169 // of the vectors p2-p1 and p3-p2.
1170 //-----------------------------------------------------------------------------
1171 TQ3Vector3D *
E3Point3D_CrossProductTri(const TQ3Point3D * p1,const TQ3Point3D * p2,const TQ3Point3D * p3,TQ3Vector3D * result)1172 E3Point3D_CrossProductTri(const TQ3Point3D *p1, const TQ3Point3D *p2, const TQ3Point3D *p3, TQ3Vector3D *result)
1173 {
1174 Q3FastPoint3D_CrossProductTri(p1, p2, p3, result);
1175 return(result);
1176 }
1177
1178
1179
1180
1181
1182 //=============================================================================
1183 // E3Triangle_CrossProductArray : Calculate a list of triangle normals.
1184 //-----------------------------------------------------------------------------
1185 TQ3Status
E3Triangle_CrossProductArray(TQ3Uns32 numTriangles,const TQ3Uns8 * usageFlags,const TQ3Uns32 * theIndices,const TQ3Point3D * thePoints,TQ3Vector3D * theNormals)1186 E3Triangle_CrossProductArray(TQ3Uns32 numTriangles,
1187 const TQ3Uns8 *usageFlags,
1188 const TQ3Uns32 *theIndices,
1189 const TQ3Point3D *thePoints,
1190 TQ3Vector3D *theNormals)
1191 { TQ3Uns32 n, m;
1192
1193
1194
1195 // Calculate the normals
1196 if (usageFlags == NULL)
1197 {
1198 for (n = 0, m = 0; n < numTriangles; n++, m += 3)
1199 {
1200 Q3Point3D_CrossProductTri(&thePoints[theIndices[m + 0]],
1201 &thePoints[theIndices[m + 1]],
1202 &thePoints[theIndices[m + 2]],
1203 &theNormals[n]);
1204 Q3Vector3D_Normalize(&theNormals[n], &theNormals[n]);
1205 }
1206 }
1207 else
1208 {
1209 for (n = 0, m = 0; n < numTriangles; n++, m += 3)
1210 {
1211 if (!usageFlags[n])
1212 {
1213 Q3Point3D_CrossProductTri(&thePoints[theIndices[m + 0]],
1214 &thePoints[theIndices[m + 1]],
1215 &thePoints[theIndices[m + 2]],
1216 &theNormals[n]);
1217 Q3Vector3D_Normalize(&theNormals[n], &theNormals[n]);
1218 }
1219 }
1220 }
1221
1222 return(kQ3Success);
1223 }
1224
1225
1226
1227
1228
1229 //=============================================================================
1230 // E3Vector2D_Length : Return length of 2D vector.
1231 //-----------------------------------------------------------------------------
1232 #pragma mark -
1233 float
E3Vector2D_Length(const TQ3Vector2D * vector2D)1234 E3Vector2D_Length(const TQ3Vector2D *vector2D)
1235 {
1236 return(Q3FastVector2D_Length(vector2D));
1237 }
1238
1239
1240
1241
1242
1243 //=============================================================================
1244 // E3Vector2D_LengthSquared : Return squared length of 2D vector.
1245 //-----------------------------------------------------------------------------
1246 float
E3Vector2D_LengthSquared(const TQ3Vector2D * vector2D)1247 E3Vector2D_LengthSquared(const TQ3Vector2D *vector2D)
1248 {
1249 return(Q3FastVector2D_LengthSquared(vector2D));
1250 }
1251
1252
1253
1254
1255
1256 //=============================================================================
1257 // E3Vector3D_Length : Return length of 3D vector.
1258 //-----------------------------------------------------------------------------
1259 float
E3Vector3D_Length(const TQ3Vector3D * vector3D)1260 E3Vector3D_Length(const TQ3Vector3D *vector3D)
1261 {
1262 return(Q3FastVector3D_Length(vector3D));
1263 }
1264
1265
1266
1267
1268
1269 //=============================================================================
1270 // E3Vector3D_LengthSquared : Return squared length of 3D vector.
1271 //-----------------------------------------------------------------------------
1272 float
E3Vector3D_LengthSquared(const TQ3Vector3D * vector3D)1273 E3Vector3D_LengthSquared(const TQ3Vector3D *vector3D)
1274 {
1275 return(Q3FastVector3D_LengthSquared(vector3D));
1276 }
1277
1278
1279
1280
1281
1282 //=============================================================================
1283 // E3Point2D_Distance : Return Euclidean distance.
1284 //-----------------------------------------------------------------------------
1285 #pragma mark -
1286 float
E3Point2D_Distance(const TQ3Point2D * p1,const TQ3Point2D * p2)1287 E3Point2D_Distance(const TQ3Point2D *p1, const TQ3Point2D *p2)
1288 {
1289 return(Q3FastPoint2D_Distance(p1, p2));
1290 }
1291
1292
1293
1294
1295
1296 //=============================================================================
1297 // E3Point2D_DistanceSquared : Return squared Euclidean distance.
1298 //-----------------------------------------------------------------------------
1299 float
E3Point2D_DistanceSquared(const TQ3Point2D * p1,const TQ3Point2D * p2)1300 E3Point2D_DistanceSquared(const TQ3Point2D *p1, const TQ3Point2D *p2)
1301 {
1302 return(Q3FastPoint2D_DistanceSquared(p1, p2));
1303 }
1304
1305
1306
1307
1308
1309 //=============================================================================
1310 // E3Param2D_Distance : Return Euclidean distance.
1311 //-----------------------------------------------------------------------------
1312 // Note : Equivalent to E3Point2D_Distance.
1313 //-----------------------------------------------------------------------------
1314 float
E3Param2D_Distance(const TQ3Param2D * p1,const TQ3Param2D * p2)1315 E3Param2D_Distance(const TQ3Param2D *p1, const TQ3Param2D *p2)
1316 {
1317 return(Q3FastParam2D_Distance(p1, p2));
1318 }
1319
1320
1321
1322
1323
1324 //=============================================================================
1325 // E3Param2D_DistanceSquared : Return squared Euclidean distance.
1326 //-----------------------------------------------------------------------------
1327 // Note : Equivalent to E3Point2D_DistanceSquared.
1328 //-----------------------------------------------------------------------------
1329 float
E3Param2D_DistanceSquared(const TQ3Param2D * p1,const TQ3Param2D * p2)1330 E3Param2D_DistanceSquared(const TQ3Param2D *p1, const TQ3Param2D *p2)
1331 {
1332 return(Q3FastParam2D_DistanceSquared(p1, p2));
1333 }
1334
1335
1336
1337
1338
1339 //=============================================================================
1340 // E3RationalPoint3D_Distance : Return Euclidean distance.
1341 //-----------------------------------------------------------------------------
1342 float
E3RationalPoint3D_Distance(const TQ3RationalPoint3D * p1,const TQ3RationalPoint3D * p2)1343 E3RationalPoint3D_Distance(const TQ3RationalPoint3D *p1, const TQ3RationalPoint3D *p2)
1344 {
1345 return(Q3FastRationalPoint3D_Distance(p1, p2));
1346 }
1347
1348
1349
1350
1351
1352 //=============================================================================
1353 // E3RationalPoint3D_DistanceSquared : Return squared Euclidean distance.
1354 //-----------------------------------------------------------------------------
1355 float
E3RationalPoint3D_DistanceSquared(const TQ3RationalPoint3D * p1,const TQ3RationalPoint3D * p2)1356 E3RationalPoint3D_DistanceSquared(const TQ3RationalPoint3D *p1, const TQ3RationalPoint3D *p2)
1357 {
1358 return(Q3FastRationalPoint3D_DistanceSquared(p1, p2));
1359 }
1360
1361
1362
1363
1364
1365 //=============================================================================
1366 // E3Point3D_Distance : Return Euclidean distance.
1367 //-----------------------------------------------------------------------------
1368 float
E3Point3D_Distance(const TQ3Point3D * p1,const TQ3Point3D * p2)1369 E3Point3D_Distance(const TQ3Point3D *p1, const TQ3Point3D *p2)
1370 {
1371 return(Q3FastPoint3D_Distance(p1, p2));
1372 }
1373
1374
1375
1376
1377
1378 //=============================================================================
1379 // E3Point3D_DistanceSquared : Return squared Euclidean distance.
1380 //-----------------------------------------------------------------------------
1381 float
E3Point3D_DistanceSquared(const TQ3Point3D * p1,const TQ3Point3D * p2)1382 E3Point3D_DistanceSquared(const TQ3Point3D *p1, const TQ3Point3D *p2)
1383 {
1384 return(Q3FastPoint3D_DistanceSquared(p1, p2));
1385 }
1386
1387
1388
1389
1390
1391 //=============================================================================
1392 // E3RationalPoint4D_Distance : Return Euclidean distance.
1393 //-----------------------------------------------------------------------------
1394 float
E3RationalPoint4D_Distance(const TQ3RationalPoint4D * p1,const TQ3RationalPoint4D * p2)1395 E3RationalPoint4D_Distance(const TQ3RationalPoint4D *p1, const TQ3RationalPoint4D *p2)
1396 {
1397 return(Q3FastRationalPoint4D_Distance(p1, p2));
1398 }
1399
1400
1401
1402
1403
1404 //=============================================================================
1405 // E3RationalPoint4D_DistanceSquared : Return squared Euclidean distance.
1406 //-----------------------------------------------------------------------------
1407 float
E3RationalPoint4D_DistanceSquared(const TQ3RationalPoint4D * p1,const TQ3RationalPoint4D * p2)1408 E3RationalPoint4D_DistanceSquared(const TQ3RationalPoint4D *p1, const TQ3RationalPoint4D *p2)
1409 {
1410 return(Q3FastRationalPoint4D_DistanceSquared(p1, p2));
1411 }
1412
1413
1414
1415
1416
1417 //=============================================================================
1418 // E3Vector2D_Negate : Scale 2D vector by -1.
1419 //-----------------------------------------------------------------------------
1420 // Note : 'result' may be the same as 'vector2D'.
1421 //-----------------------------------------------------------------------------
1422 #pragma mark -
1423 TQ3Vector2D *
E3Vector2D_Negate(const TQ3Vector2D * vector2D,TQ3Vector2D * result)1424 E3Vector2D_Negate(const TQ3Vector2D *vector2D, TQ3Vector2D *result)
1425 {
1426 Q3FastVector2D_Negate(vector2D, result);
1427 return(result);
1428 }
1429
1430
1431
1432
1433
1434 //=============================================================================
1435 // E3Vector3D_Negate : Scale 3D vector vector by -1.
1436 //-----------------------------------------------------------------------------
1437 // Note : 'result' may be the same as 'vector3D'.
1438 //-----------------------------------------------------------------------------
1439 TQ3Vector3D *
E3Vector3D_Negate(const TQ3Vector3D * vector3D,TQ3Vector3D * result)1440 E3Vector3D_Negate(const TQ3Vector3D *vector3D, TQ3Vector3D *result)
1441 {
1442 Q3FastVector3D_Negate(vector3D, result);
1443 return(result);
1444 }
1445
1446
1447
1448
1449
1450 //=============================================================================
1451 // E3Vector2D_Scale : Scale 2D vector by the given factor.
1452 //-----------------------------------------------------------------------------
1453 // Note : 'result' may be the same as 'vector2D'.
1454 //-----------------------------------------------------------------------------
1455 #pragma mark -
1456 TQ3Vector2D *
E3Vector2D_Scale(const TQ3Vector2D * vector2D,float scalar,TQ3Vector2D * result)1457 E3Vector2D_Scale(const TQ3Vector2D *vector2D, float scalar, TQ3Vector2D *result)
1458 {
1459 Q3FastVector2D_Scale(vector2D, scalar, result);
1460 return(result);
1461 }
1462
1463
1464
1465
1466
1467 //=============================================================================
1468 // E3Vector3D_Scale : Scale 3D vector by the given factor.
1469 //-----------------------------------------------------------------------------
1470 // Note : 'result' may be the same as 'vector3D'.
1471 //-----------------------------------------------------------------------------
1472 TQ3Vector3D *
E3Vector3D_Scale(const TQ3Vector3D * vector3D,float scalar,TQ3Vector3D * result)1473 E3Vector3D_Scale(const TQ3Vector3D *vector3D, float scalar, TQ3Vector3D *result)
1474 {
1475 Q3FastVector3D_Scale(vector3D, scalar, result);
1476 return(result);
1477 }
1478
1479
1480
1481
1482
1483 //=============================================================================
1484 // E3Vector2D_Normalize : Scale 2D vector to length 1.
1485 //-----------------------------------------------------------------------------
1486 // Note : 'result' may be the same as 'vector2D'.
1487 // To obtain valid results, |vector2D| must not be 0.
1488 //-----------------------------------------------------------------------------
1489 #pragma mark -
1490 TQ3Vector2D *
E3Vector2D_Normalize(const TQ3Vector2D * vector2D,TQ3Vector2D * result)1491 E3Vector2D_Normalize(const TQ3Vector2D *vector2D, TQ3Vector2D *result)
1492 {
1493 Q3FastVector2D_Normalize(vector2D, result);
1494 return(result);
1495 }
1496
1497
1498
1499
1500
1501 //=============================================================================
1502 // E3Vector3D_Normalize : Scale 3D vector to length 1.
1503 //-----------------------------------------------------------------------------
1504 // Note : 'result' may be the same as 'vector3D'.
1505 // To obtain valid results, |vector2D| must not be 0.
1506 //-----------------------------------------------------------------------------
1507 TQ3Vector3D *
E3Vector3D_Normalize(const TQ3Vector3D * vector3D,TQ3Vector3D * result)1508 E3Vector3D_Normalize(const TQ3Vector3D *vector3D, TQ3Vector3D *result)
1509 {
1510 Q3FastVector3D_Normalize(vector3D, result);
1511 return(result);
1512 }
1513
1514
1515
1516
1517
1518 //=============================================================================
1519 // E3Vector2D_Add : Add two 2D vectors.
1520 //-----------------------------------------------------------------------------
1521 // Note : 'result' may be the same as 'v1' and/or 'v2'.
1522 //-----------------------------------------------------------------------------
1523 #pragma mark -
1524 TQ3Vector2D *
E3Vector2D_Add(const TQ3Vector2D * v1,const TQ3Vector2D * v2,TQ3Vector2D * result)1525 E3Vector2D_Add(const TQ3Vector2D *v1, const TQ3Vector2D *v2, TQ3Vector2D *result)
1526 {
1527 Q3FastVector2D_Add(v1, v2, result);
1528 return(result);
1529 }
1530
1531
1532
1533
1534
1535 //=============================================================================
1536 // E3Vector3D_Add : Add two 3D vectors.
1537 //-----------------------------------------------------------------------------
1538 // Note : 'result' may be the same as 'v1' and/or 'v2'.
1539 //-----------------------------------------------------------------------------
1540 TQ3Vector3D *
E3Vector3D_Add(const TQ3Vector3D * v1,const TQ3Vector3D * v2,TQ3Vector3D * result)1541 E3Vector3D_Add(const TQ3Vector3D *v1, const TQ3Vector3D *v2, TQ3Vector3D *result)
1542 {
1543 Q3FastVector3D_Add(v1, v2, result);
1544 return(result);
1545 }
1546
1547
1548
1549
1550
1551 //=============================================================================
1552 // E3Vector2D_Subtract : Subtract 2D vector v2 from v1.
1553 //-----------------------------------------------------------------------------
1554 // Note : 'result' may be the same as 'v1' and/or 'v2'.
1555 //-----------------------------------------------------------------------------
1556 TQ3Vector2D *
E3Vector2D_Subtract(const TQ3Vector2D * v1,const TQ3Vector2D * v2,TQ3Vector2D * result)1557 E3Vector2D_Subtract(const TQ3Vector2D *v1, const TQ3Vector2D *v2, TQ3Vector2D *result)
1558 {
1559 Q3FastVector2D_Subtract(v1, v2, result);
1560 return(result);
1561 }
1562
1563
1564
1565
1566
1567 //=============================================================================
1568 // E3Vector3D_Subtract : Subtract 3D vector v2 from v1.
1569 //-----------------------------------------------------------------------------
1570 // Note : 'result' may be the same as 'v1' and/or 'v2'.
1571 //-----------------------------------------------------------------------------
1572 TQ3Vector3D *
E3Vector3D_Subtract(const TQ3Vector3D * v1,const TQ3Vector3D * v2,TQ3Vector3D * result)1573 E3Vector3D_Subtract(const TQ3Vector3D *v1, const TQ3Vector3D *v2, TQ3Vector3D *result)
1574 {
1575 Q3FastVector3D_Subtract(v1, v2, result);
1576 return(result);
1577 }
1578
1579
1580
1581
1582
1583 //=============================================================================
1584 // E3Point2D_Vector2D_Add : Add 2D vector to point.
1585 //-----------------------------------------------------------------------------
1586 // Note : 'result' may be the same as 'point2D'.
1587 //-----------------------------------------------------------------------------
1588 #pragma mark -
1589 TQ3Point2D *
E3Point2D_Vector2D_Add(const TQ3Point2D * point2D,const TQ3Vector2D * vector2D,TQ3Point2D * result)1590 E3Point2D_Vector2D_Add(const TQ3Point2D *point2D, const TQ3Vector2D *vector2D, TQ3Point2D *result)
1591 {
1592 Q3FastPoint2D_Vector2D_Add(point2D, vector2D, result);
1593 return(result);
1594 }
1595
1596
1597
1598
1599
1600 //=============================================================================
1601 // E3Param2D_Vector2D_Add : Add 2D vector to parametric point.
1602 //-----------------------------------------------------------------------------
1603 // Note : Equivalent to E3Point2D_Vector2D_Add.
1604 //-----------------------------------------------------------------------------
1605 TQ3Param2D *
E3Param2D_Vector2D_Add(const TQ3Param2D * param2D,const TQ3Vector2D * vector2D,TQ3Param2D * result)1606 E3Param2D_Vector2D_Add(const TQ3Param2D *param2D, const TQ3Vector2D *vector2D, TQ3Param2D *result)
1607 {
1608 Q3FastParam2D_Vector2D_Add(param2D, vector2D, result);
1609 return(result);
1610 }
1611
1612
1613
1614
1615
1616 //=============================================================================
1617 // E3Point3D_Vector3D_Add : Add 3D vector to point.
1618 //-----------------------------------------------------------------------------
1619 // Note : 'result' may be the same as 'point3D'.
1620 //-----------------------------------------------------------------------------
1621 TQ3Point3D *
E3Point3D_Vector3D_Add(const TQ3Point3D * point3D,const TQ3Vector3D * vector3D,TQ3Point3D * result)1622 E3Point3D_Vector3D_Add(const TQ3Point3D *point3D, const TQ3Vector3D *vector3D, TQ3Point3D *result)
1623 {
1624 Q3FastPoint3D_Vector3D_Add(point3D, vector3D, result);
1625 return(result);
1626 }
1627
1628
1629
1630
1631
1632 //=============================================================================
1633 // E3Point2D_Vector2D_Subtract : Subtract 2D vector from point.
1634 //-----------------------------------------------------------------------------
1635 // Note : 'result' may be the same as 'point2D'.
1636 //-----------------------------------------------------------------------------
1637 TQ3Point2D *
E3Point2D_Vector2D_Subtract(const TQ3Point2D * point2D,const TQ3Vector2D * vector2D,TQ3Point2D * result)1638 E3Point2D_Vector2D_Subtract(const TQ3Point2D *point2D, const TQ3Vector2D *vector2D, TQ3Point2D *result)
1639 {
1640 Q3FastPoint2D_Vector2D_Subtract(point2D, vector2D, result);
1641 return(result);
1642 }
1643
1644
1645
1646
1647
1648 //=============================================================================
1649 // E3Param2D_Vector2D_Subtract : Subtract 2D vector from parametric point.
1650 //-----------------------------------------------------------------------------
1651 // Note : Equivalent to E3Point2D_Vector2D_Subtract.
1652 //-----------------------------------------------------------------------------
1653 TQ3Param2D *
E3Param2D_Vector2D_Subtract(const TQ3Param2D * param2D,const TQ3Vector2D * vector2D,TQ3Param2D * result)1654 E3Param2D_Vector2D_Subtract(const TQ3Param2D *param2D, const TQ3Vector2D *vector2D, TQ3Param2D *result)
1655 {
1656 Q3FastParam2D_Vector2D_Subtract(param2D, vector2D, result);
1657 return(result);
1658 }
1659
1660
1661
1662
1663
1664 //=============================================================================
1665 // E3Point3D_Vector3D_Subtract : Subtract 3D vector from point.
1666 //-----------------------------------------------------------------------------
1667 // Note : 'result' may be the same as 'point3D'.
1668 //-----------------------------------------------------------------------------
1669 TQ3Point3D *
E3Point3D_Vector3D_Subtract(const TQ3Point3D * point3D,const TQ3Vector3D * vector3D,TQ3Point3D * result)1670 E3Point3D_Vector3D_Subtract(const TQ3Point3D *point3D, const TQ3Vector3D *vector3D, TQ3Point3D *result)
1671 {
1672 Q3FastPoint3D_Vector3D_Subtract(point3D, vector3D, result);
1673 return(result);
1674 }
1675
1676
1677
1678
1679
1680 //=============================================================================
1681 // E3Point2D_Subtract : Subtract 2D point p2 from p1.
1682 //-----------------------------------------------------------------------------
1683 // Note : 'result' may be the same as 'p1' and/or 'p2'.
1684 //-----------------------------------------------------------------------------
1685 #pragma mark -
1686 TQ3Vector2D *
E3Point2D_Subtract(const TQ3Point2D * p1,const TQ3Point2D * p2,TQ3Vector2D * result)1687 E3Point2D_Subtract(const TQ3Point2D *p1, const TQ3Point2D *p2, TQ3Vector2D *result)
1688 {
1689 Q3FastPoint2D_Subtract(p1, p2, result);
1690 return(result);
1691 }
1692
1693
1694
1695
1696
1697 //=============================================================================
1698 // E3Param2D_Subtract : Subtract 2D point parametric p2 from p1.
1699 //-----------------------------------------------------------------------------
1700 // Note : Equivalent to E3Point2D_Subtract.
1701 //-----------------------------------------------------------------------------
1702 TQ3Vector2D *
E3Param2D_Subtract(const TQ3Param2D * p1,const TQ3Param2D * p2,TQ3Vector2D * result)1703 E3Param2D_Subtract(const TQ3Param2D *p1, const TQ3Param2D *p2, TQ3Vector2D *result)
1704 {
1705 Q3FastParam2D_Subtract(p1, p2, result);
1706 return(result);
1707 }
1708
1709
1710
1711
1712
1713 //=============================================================================
1714 // E3Point3D_Subtract : Subtract 3D point p2 from p1.
1715 //-----------------------------------------------------------------------------
1716 // Note : 'result' may be the same as 'p1' and/or 'p2'.
1717 //-----------------------------------------------------------------------------
1718 TQ3Vector3D *
E3Point3D_Subtract(const TQ3Point3D * p1,const TQ3Point3D * p2,TQ3Vector3D * result)1719 E3Point3D_Subtract(const TQ3Point3D *p1, const TQ3Point3D *p2, TQ3Vector3D *result)
1720 {
1721 Q3FastPoint3D_Subtract(p1, p2, result);
1722 return(result);
1723 }
1724
1725
1726
1727
1728 //=============================================================================
1729 // E3Point2D_RRatio : Return point at ratio r2/(r1+r2) along segment from
1730 // p1 to p2.
1731 //-----------------------------------------------------------------------------
1732 // Note : 'result' may be the same as 'p1' and/or 'p2'.
1733 //
1734 // The QD3D docs claim that the ratio used is r1/(r1+r2), but
1735 // we found by direct experimentation that the QD3D library (1.6)
1736 // in fact uses r2/(r1+r2) instead.
1737 //
1738 // As usual, we do as QD3D does, not as the docs say.
1739 //-----------------------------------------------------------------------------
1740 #pragma mark -
1741 TQ3Point2D *
E3Point2D_RRatio(const TQ3Point2D * p1,const TQ3Point2D * p2,float r1,float r2,TQ3Point2D * result)1742 E3Point2D_RRatio(const TQ3Point2D *p1, const TQ3Point2D *p2, float r1, float r2, TQ3Point2D *result)
1743 {
1744 Q3FastPoint2D_RRatio(p1, p2, r1, r2, result);
1745 return(result);
1746 }
1747
1748
1749
1750
1751
1752 //=============================================================================
1753 // E3Param2D_RRatio : Return point at ratio r2/(r1+r2) along segment from
1754 // p1 to p2.
1755 //-----------------------------------------------------------------------------
1756 // Note : Equivalent to E3Point2D_RRatio.
1757 //-----------------------------------------------------------------------------
1758 TQ3Param2D *
E3Param2D_RRatio(const TQ3Param2D * p1,const TQ3Param2D * p2,float r1,float r2,TQ3Param2D * result)1759 E3Param2D_RRatio(const TQ3Param2D *p1, const TQ3Param2D *p2, float r1, float r2, TQ3Param2D *result)
1760 {
1761 Q3FastParam2D_RRatio(p1, p2, r1, r2, result);
1762 return(result);
1763 }
1764
1765
1766
1767
1768
1769 //=============================================================================
1770 // E3Point3D_RRatio : Return point at ratio r2/(r1+r2) along segment from
1771 // p1 to p2.
1772 //-----------------------------------------------------------------------------
1773 // Note : 'result' may be the same as 'p1' and/or 'p2'.
1774 //
1775 // The QD3D docs claim that the ratio used is r1/(r1+r2), but
1776 // we found by direct experimentation that the QD3D library (1.6)
1777 // in fact uses r2/(r1+r2) instead.
1778 //
1779 // As usual, we do as QD3D does, not as the docs say.
1780 //-----------------------------------------------------------------------------
1781 TQ3Point3D *
E3Point3D_RRatio(const TQ3Point3D * p1,const TQ3Point3D * p2,float r1,float r2,TQ3Point3D * result)1782 E3Point3D_RRatio(const TQ3Point3D *p1, const TQ3Point3D *p2, float r1, float r2, TQ3Point3D *result)
1783 {
1784 Q3FastPoint3D_RRatio(p1, p2, r1, r2, result);
1785 return(result);
1786 }
1787
1788
1789
1790
1791
1792 //=============================================================================
1793 // E3RationalPoint4D_RRatio : Return point at ratio r2/(r1+r2) along
1794 // segment from p1 to p2.
1795 //-----------------------------------------------------------------------------
1796 // Note : 'result' may be the same as 'p1' and/or 'p2'.
1797 //
1798 // The QD3D docs claim that the ratio used is r1/(r1+r2), but
1799 // we found by direct experimentation that the QD3D library (1.6)
1800 // in fact uses r2/(r1+r2) instead.
1801 //
1802 // As usual, we do as QD3D does, not as the docs say.
1803 //-----------------------------------------------------------------------------
1804 TQ3RationalPoint4D *
E3RationalPoint4D_RRatio(const TQ3RationalPoint4D * p1,const TQ3RationalPoint4D * p2,float r1,float r2,TQ3RationalPoint4D * result)1805 E3RationalPoint4D_RRatio(const TQ3RationalPoint4D *p1, const TQ3RationalPoint4D *p2,
1806 float r1, float r2, TQ3RationalPoint4D *result)
1807 {
1808 Q3FastRationalPoint4D_RRatio(p1, p2, r1, r2, result);
1809 return(result);
1810 }
1811
1812
1813
1814
1815
1816 //=============================================================================
1817 // E3Point2D_AffineComb : Return weighted combination of several 2D points.
1818 //-----------------------------------------------------------------------------
1819 // Note : Weights are NOT required to sum to 1, but we they must not sum
1820 // to 0.
1821 //-----------------------------------------------------------------------------
1822 #pragma mark -
1823 TQ3Point2D *
E3Point2D_AffineComb(const TQ3Point2D * points2D,const float * weights,TQ3Uns32 numPoints,TQ3Point2D * result)1824 E3Point2D_AffineComb(const TQ3Point2D *points2D,
1825 const float *weights,
1826 TQ3Uns32 numPoints,
1827 TQ3Point2D *result)
1828 {
1829 float x = 0.0f;
1830 float y = 0.0f;
1831 float totalWeight = 0.0f;
1832 TQ3Uns32 i;
1833
1834 for (i = 0; i < numPoints; ++i)
1835 {
1836 x += points2D[i].x * weights[i];
1837 y += points2D[i].y * weights[i];
1838 totalWeight += weights[i];
1839 }
1840
1841 result->x = x / totalWeight;
1842 result->y = y / totalWeight;
1843
1844 return(result);
1845 }
1846
1847
1848
1849
1850
1851 //=============================================================================
1852 // E3Param2D_AffineComb : Return weighted combination of several 2D
1853 // parametric points.
1854 //-----------------------------------------------------------------------------
1855 // Note : Equivalent to E3Point2D_AffineComb.
1856 //-----------------------------------------------------------------------------
1857 TQ3Param2D *
E3Param2D_AffineComb(const TQ3Param2D * params2D,const float * weights,TQ3Uns32 numPoints,TQ3Param2D * result)1858 E3Param2D_AffineComb(const TQ3Param2D *params2D,
1859 const float *weights,
1860 TQ3Uns32 numPoints,
1861 TQ3Param2D *result)
1862 {
1863 return((TQ3Param2D*) E3Point2D_AffineComb((const TQ3Point2D*) params2D, weights,
1864 numPoints, (TQ3Point2D*) result));
1865 }
1866
1867
1868
1869
1870
1871 //=============================================================================
1872 // E3RationalPoint3D_AffineComb : Return weighted combination of several
1873 // 3D rational points.
1874 //-----------------------------------------------------------------------------
1875 // Note : Weights are NOT required to sum to 1, but we they must not sum
1876 // to 0.
1877 //-----------------------------------------------------------------------------
1878 TQ3RationalPoint3D *
E3RationalPoint3D_AffineComb(const TQ3RationalPoint3D * rationalPoints3D,const float * weights,TQ3Uns32 numPoints,TQ3RationalPoint3D * result)1879 E3RationalPoint3D_AffineComb(const TQ3RationalPoint3D *rationalPoints3D,
1880 const float *weights,
1881 TQ3Uns32 numPoints,
1882 TQ3RationalPoint3D *result)
1883 {
1884 float x = 0.0f;
1885 float y = 0.0f;
1886 float totalWeight = 0.0f;
1887 TQ3Uns32 i;
1888
1889 for (i = 0; i < numPoints; ++i)
1890 {
1891 x += rationalPoints3D[i].x / rationalPoints3D[i].w * weights[i];
1892 y += rationalPoints3D[i].y / rationalPoints3D[i].w * weights[i];
1893 totalWeight += weights[i];
1894 }
1895
1896 result->x = x / totalWeight;
1897 result->y = y / totalWeight;
1898 result->w = 1.0f;
1899
1900 return(result);
1901 }
1902
1903
1904
1905
1906
1907 //=============================================================================
1908 // E3Point3D_AffineComb : Return weighted combination of several 3D points.
1909 //-----------------------------------------------------------------------------
1910 // Note : Weights are NOT required to sum to 1, but we they must not sum
1911 // to 0.
1912 //-----------------------------------------------------------------------------
1913 TQ3Point3D *
E3Point3D_AffineComb(const TQ3Point3D * points3D,const float * weights,TQ3Uns32 numPoints,TQ3Point3D * result)1914 E3Point3D_AffineComb(const TQ3Point3D *points3D,
1915 const float *weights,
1916 TQ3Uns32 numPoints,
1917 TQ3Point3D *result)
1918 {
1919 float x = 0.0f;
1920 float y = 0.0f;
1921 float z = 0.0f;
1922 float totalWeight = 0.0f;
1923 TQ3Uns32 i;
1924
1925 for (i = 0; i < numPoints; ++i)
1926 {
1927 x += points3D[i].x * weights[i];
1928 y += points3D[i].y * weights[i];
1929 z += points3D[i].z * weights[i];
1930 totalWeight += weights[i];
1931 }
1932
1933 result->x = x / totalWeight;
1934 result->y = y / totalWeight;
1935 result->z = z / totalWeight;
1936
1937 return(result);
1938 }
1939
1940
1941
1942
1943
1944 //=============================================================================
1945 // E3RationalPoint4D_AffineComb : Return weighted combination of several
1946 // 4D rational points.
1947 //-----------------------------------------------------------------------------
1948 // Note : Weights are NOT required to sum to 1, but we they must not sum
1949 // to 0.
1950 //-----------------------------------------------------------------------------
1951 TQ3RationalPoint4D *
E3RationalPoint4D_AffineComb(const TQ3RationalPoint4D * rationalPoints4D,const float * weights,TQ3Uns32 numPoints,TQ3RationalPoint4D * result)1952 E3RationalPoint4D_AffineComb(const TQ3RationalPoint4D *rationalPoints4D,
1953 const float *weights,
1954 TQ3Uns32 numPoints,
1955 TQ3RationalPoint4D *result)
1956 {
1957 float x = 0.0f;
1958 float y = 0.0f;
1959 float z = 0.0f;
1960 float totalWeight = 0.0f;
1961 TQ3Uns32 i;
1962
1963 for (i = 0; i < numPoints; ++i)
1964 {
1965 x += rationalPoints4D[i].x / rationalPoints4D[i].w * weights[i];
1966 y += rationalPoints4D[i].y / rationalPoints4D[i].w * weights[i];
1967 z += rationalPoints4D[i].z / rationalPoints4D[i].w * weights[i];
1968 totalWeight += weights[i];
1969 }
1970
1971 result->x = x / totalWeight;
1972 result->y = y / totalWeight;
1973 result->z = z / totalWeight;
1974 result->w = 1.0f;
1975
1976 return(result);
1977 }
1978
1979
1980
1981
1982
1983 //=============================================================================
1984 // E3Vector2D_Transform : Transform 2D vector by 3x3 matrix.
1985 //-----------------------------------------------------------------------------
1986 // Note : 'result' may be the same as 'vector2D'.
1987 //
1988 // Note that the translation and perspective components of the
1989 // matrix is ignored (as if it were really a 2x2 matrix).
1990 //
1991 // Contrast with E3Point2D_Transform, which does the full 3x3
1992 // transformation.
1993 //-----------------------------------------------------------------------------
1994 #pragma mark -
1995 TQ3Vector2D *
E3Vector2D_Transform(const TQ3Vector2D * vector2D,const TQ3Matrix3x3 * matrix3x3,TQ3Vector2D * result)1996 E3Vector2D_Transform(const TQ3Vector2D *vector2D, const TQ3Matrix3x3 *matrix3x3,
1997 TQ3Vector2D *result)
1998 {
1999 // Save input to avoid problems when result is same as input
2000 float x = vector2D->x;
2001 float y = vector2D->y;
2002
2003 #define M(x,y) matrix3x3->value[x][y]
2004 result->x = x*M(0,0) + y*M(1,0);
2005 result->y = x*M(0,1) + y*M(1,1);
2006 #undef M
2007
2008 return(result);
2009 }
2010
2011
2012
2013
2014
2015 //=============================================================================
2016 // E3Vector3D_Transform : Transform 3D vector by 4x4 matrix.
2017 //-----------------------------------------------------------------------------
2018 // Note : 'result' may be the same as 'vector3D'.
2019 //
2020 // Note that the translation and perspective components of the
2021 // matrix is ignored (as if it were really a 3x3 matrix).
2022 //
2023 // Contrast with E3Point3D_Transform, which does the full 4x4
2024 // transformation.
2025 //-----------------------------------------------------------------------------
2026 TQ3Vector3D *
E3Vector3D_Transform(const TQ3Vector3D * vector3D,const TQ3Matrix4x4 * matrix4x4,TQ3Vector3D * result)2027 E3Vector3D_Transform(const TQ3Vector3D *vector3D, const TQ3Matrix4x4 *matrix4x4,
2028 TQ3Vector3D *result)
2029 {
2030 // Save input to avoid problems when result is same as input
2031 float x = vector3D->x;
2032 float y = vector3D->y;
2033 float z = vector3D->z;
2034
2035 #define M(x,y) matrix4x4->value[x][y]
2036 result->x = x*M(0,0) + y*M(1,0) + z*M(2,0);
2037 result->y = x*M(0,1) + y*M(1,1) + z*M(2,1);
2038 result->z = x*M(0,2) + y*M(1,2) + z*M(2,2);
2039 #undef M
2040
2041 return(result);
2042 }
2043
2044
2045
2046
2047
2048 //=============================================================================
2049 // E3Point2D_Transform : Transform 2D point by 3x3 matrix.
2050 //-----------------------------------------------------------------------------
2051 // Note : 'result' may be the same as 'point2D'.
2052 //-----------------------------------------------------------------------------
2053 TQ3Point2D *
E3Point2D_Transform(const TQ3Point2D * point2D,const TQ3Matrix3x3 * matrix3x3,TQ3Point2D * result)2054 E3Point2D_Transform(const TQ3Point2D *point2D, const TQ3Matrix3x3 *matrix3x3,
2055 TQ3Point2D *result)
2056 {
2057 // Save input to avoid problems when result is same as input
2058 float x = point2D->x;
2059 float y = point2D->y;
2060 float neww;
2061
2062 #define M(x,y) matrix3x3->value[x][y]
2063 result->x = x*M(0,0) + y*M(1,0) + M(2,0);
2064 result->y = x*M(0,1) + y*M(1,1) + M(2,1);
2065 neww = x*M(0,2) + y*M(1,2) + M(2,2);
2066 #undef M
2067
2068 if (neww == 0.0f)
2069 {
2070 E3ErrorManager_PostError( kQ3ErrorInfiniteRationalPoint, kQ3False );
2071 neww = 1.0f;
2072 }
2073
2074 if (neww != 1.0f)
2075 {
2076 float invw = 1.0f / neww;
2077 result->x *= invw;
2078 result->y *= invw;
2079 }
2080
2081 return(result);
2082 }
2083
2084
2085
2086
2087
2088 //=============================================================================
2089 // E3Param2D_Transform : Transform 2D parametric point by 3x3 matrix.
2090 //-----------------------------------------------------------------------------
2091 // Note : Equivalent to E3Point2D_Transform.
2092 //-----------------------------------------------------------------------------
2093 TQ3Param2D *
E3Param2D_Transform(const TQ3Param2D * param2D,const TQ3Matrix3x3 * matrix3x3,TQ3Param2D * result)2094 E3Param2D_Transform(const TQ3Param2D *param2D, const TQ3Matrix3x3 *matrix3x3,
2095 TQ3Param2D *result)
2096 {
2097 return((TQ3Param2D*) E3Point2D_Transform((const TQ3Point2D*) param2D, matrix3x3,
2098 (TQ3Point2D*) result));
2099 }
2100
2101
2102
2103
2104
2105 //=============================================================================
2106 // E3RationalPoint3D_Transform : Transform 3D rational point by 3x3 matrix.
2107 //-----------------------------------------------------------------------------
2108 // Note : 'result' may be the same as 'point3D'.
2109 //-----------------------------------------------------------------------------
2110 TQ3RationalPoint3D *
E3RationalPoint3D_Transform(const TQ3RationalPoint3D * rationalPoint3D,const TQ3Matrix3x3 * matrix3x3,TQ3RationalPoint3D * result)2111 E3RationalPoint3D_Transform(const TQ3RationalPoint3D *rationalPoint3D,
2112 const TQ3Matrix3x3 *matrix3x3, TQ3RationalPoint3D *result)
2113 {
2114 // Save input to avoid problems when result is same as input
2115 float x = rationalPoint3D->x;
2116 float y = rationalPoint3D->y;
2117 float w = rationalPoint3D->w;
2118
2119 #define M(x,y) matrix3x3->value[x][y]
2120 result->x = x*M(0,0) + y*M(1,0) + w*M(2,0);
2121 result->y = x*M(0,1) + y*M(1,1) + w*M(2,1);
2122 result->w = x*M(0,2) + y*M(1,2) + w*M(2,2);
2123 #undef M
2124
2125 return(result);
2126 }
2127
2128
2129
2130
2131
2132 //=============================================================================
2133 // E3Point3D_Transform : Transform 3D point by 4x4 matrix.
2134 //-----------------------------------------------------------------------------
2135 // Note : 'result' may be the same as 'point3D'.
2136 //-----------------------------------------------------------------------------
2137 TQ3Point3D *
E3Point3D_Transform(const TQ3Point3D * point3D,const TQ3Matrix4x4 * matrix4x4,TQ3Point3D * result)2138 E3Point3D_Transform(const TQ3Point3D *point3D, const TQ3Matrix4x4 *matrix4x4,
2139 TQ3Point3D *result)
2140 {
2141 // Save input to avoid problems when result is same as input
2142 float x = point3D->x;
2143 float y = point3D->y;
2144 float z = point3D->z;
2145 float neww;
2146
2147 #define M(x,y) matrix4x4->value[x][y]
2148 result->x = x*M(0,0) + y*M(1,0) + z*M(2,0) + M(3,0);
2149 result->y = x*M(0,1) + y*M(1,1) + z*M(2,1) + M(3,1);
2150 result->z = x*M(0,2) + y*M(1,2) + z*M(2,2) + M(3,2);
2151 neww = x*M(0,3) + y*M(1,3) + z*M(2,3) + M(3,3);
2152 #undef M
2153
2154 if (neww == 0.0f)
2155 {
2156 E3ErrorManager_PostError( kQ3ErrorInfiniteRationalPoint, kQ3False );
2157 neww = 1.0f;
2158 }
2159
2160 if (neww != 1.0f)
2161 {
2162 float invw = 1.0f / neww;
2163 result->x *= invw;
2164 result->y *= invw;
2165 result->z *= invw;
2166 }
2167
2168 return(result);
2169 }
2170
2171
2172
2173
2174
2175 //=============================================================================
2176 // E3RationalPoint4D_Transform : Transform 4D rational point by 4x4 matrix.
2177 //-----------------------------------------------------------------------------
2178 // Note : 'result' may be the same as 'point4D'.
2179 //-----------------------------------------------------------------------------
2180 TQ3RationalPoint4D *
E3RationalPoint4D_Transform(const TQ3RationalPoint4D * rationalPoint4D,const TQ3Matrix4x4 * matrix4x4,TQ3RationalPoint4D * result)2181 E3RationalPoint4D_Transform(const TQ3RationalPoint4D *rationalPoint4D,
2182 const TQ3Matrix4x4 *matrix4x4, TQ3RationalPoint4D *result)
2183 {
2184 // Save input to avoid problems when result is same as input
2185 float x = rationalPoint4D->x;
2186 float y = rationalPoint4D->y;
2187 float z = rationalPoint4D->z;
2188 float w = rationalPoint4D->w;
2189
2190 #define M(x,y) matrix4x4->value[x][y]
2191 result->x = x*M(0,0) + y*M(1,0) + z*M(2,0) + w*M(3,0);
2192 result->y = x*M(0,1) + y*M(1,1) + z*M(2,1) + w*M(3,1);
2193 result->z = x*M(0,2) + y*M(1,2) + z*M(2,2) + w*M(3,2);
2194 result->w = x*M(0,3) + y*M(1,3) + z*M(2,3) + w*M(3,3);
2195 #undef M
2196
2197 return(result);
2198 }
2199
2200
2201
2202
2203
2204 //=============================================================================
2205 // E3Vector2D_To2DTransformArray : Transform array of 2D vectors by 3x3 matrix.
2206 //-----------------------------------------------------------------------------
2207 // Note : 'outVectors2D' may be the same as 'inVectors2D'.
2208 //-----------------------------------------------------------------------------
2209 TQ3Status
E3Vector2D_To2DTransformArray(const TQ3Vector2D * inVectors2D,const TQ3Matrix3x3 * matrix3x3,TQ3Vector2D * outVectors2D,TQ3Uns32 numVectors,TQ3Uns32 inStructSize,TQ3Uns32 outStructSize)2210 E3Vector2D_To2DTransformArray(const TQ3Vector2D *inVectors2D,
2211 const TQ3Matrix3x3 *matrix3x3,
2212 TQ3Vector2D *outVectors2D,
2213 TQ3Uns32 numVectors,
2214 TQ3Uns32 inStructSize,
2215 TQ3Uns32 outStructSize)
2216 {
2217 TQ3Uns32 i;
2218
2219 for (i = 0; i < numVectors; ++i)
2220 {
2221 E3Vector2D_Transform(inVectors2D, matrix3x3, outVectors2D);
2222
2223 ((const char*&) inVectors2D) += inStructSize;
2224 ((char*&) outVectors2D) += outStructSize;
2225 }
2226
2227 return(kQ3Success);
2228 }
2229
2230
2231
2232
2233
2234 //=============================================================================
2235 // E3Vector3D_To3DTransformArray : Transform array of 3D vectors by 4x4 matrix.
2236 //-----------------------------------------------------------------------------
2237 // Note : 'outVectors3D' may be the same as 'inVectors3D'.
2238 //-----------------------------------------------------------------------------
2239 TQ3Status
E3Vector3D_To3DTransformArray(const TQ3Vector3D * inVectors3D,const TQ3Matrix4x4 * matrix4x4,TQ3Vector3D * outVectors3D,TQ3Uns32 numVectors,TQ3Uns32 inStructSize,TQ3Uns32 outStructSize)2240 E3Vector3D_To3DTransformArray(const TQ3Vector3D *inVectors3D,
2241 const TQ3Matrix4x4 *matrix4x4,
2242 TQ3Vector3D *outVectors3D,
2243 TQ3Uns32 numVectors,
2244 TQ3Uns32 inStructSize,
2245 TQ3Uns32 outStructSize)
2246 {
2247 TQ3Uns32 i;
2248
2249 for (i = 0; i < numVectors; ++i)
2250 {
2251 E3Vector3D_Transform(inVectors3D, matrix4x4, outVectors3D);
2252
2253 ((const char*&) inVectors3D) += inStructSize;
2254 ((char*&) outVectors3D) += outStructSize;
2255 }
2256
2257 return(kQ3Success);
2258 }
2259
2260
2261
2262
2263
2264 //=============================================================================
2265 // E3Point2D_To2DTransformArray : Transform array of 2D points by 3x3 matrix.
2266 //-----------------------------------------------------------------------------
2267 // Note : 'outPoints2D' may be the same as 'inPoints2D'.
2268 //-----------------------------------------------------------------------------
2269 TQ3Status
E3Point2D_To2DTransformArray(const TQ3Point2D * inPoints2D,const TQ3Matrix3x3 * matrix3x3,TQ3Point2D * outPoints2D,TQ3Uns32 numPoints,TQ3Uns32 inStructSize,TQ3Uns32 outStructSize)2270 E3Point2D_To2DTransformArray(const TQ3Point2D *inPoints2D,
2271 const TQ3Matrix3x3 *matrix3x3,
2272 TQ3Point2D *outPoints2D,
2273 TQ3Uns32 numPoints,
2274 TQ3Uns32 inStructSize,
2275 TQ3Uns32 outStructSize)
2276 {
2277 TQ3Uns32 i;
2278
2279 for (i = 0; i < numPoints; ++i)
2280 {
2281 E3Point2D_Transform(inPoints2D, matrix3x3, outPoints2D);
2282
2283 ((const char*&) inPoints2D) += inStructSize;
2284 ((char*&) outPoints2D) += outStructSize;
2285 }
2286
2287 return(kQ3Success);
2288 }
2289
2290
2291
2292
2293
2294 //=============================================================================
2295 // E3Point2D_To3DTransformArray : Transform array of 2D points by 3x3
2296 // matrix into 3D rational points.
2297 //-----------------------------------------------------------------------------
2298 TQ3Status
E3Point2D_To3DTransformArray(const TQ3Point2D * inPoints2D,const TQ3Matrix3x3 * matrix3x3,TQ3RationalPoint3D * outRationalPoints3D,TQ3Uns32 numPoints,TQ3Uns32 inStructSize,TQ3Uns32 outStructSize)2299 E3Point2D_To3DTransformArray(const TQ3Point2D *inPoints2D,
2300 const TQ3Matrix3x3 *matrix3x3,
2301 TQ3RationalPoint3D *outRationalPoints3D,
2302 TQ3Uns32 numPoints,
2303 TQ3Uns32 inStructSize,
2304 TQ3Uns32 outStructSize)
2305 {
2306 TQ3Uns32 i;
2307
2308 for (i = 0; i < numPoints; ++i)
2309 {
2310 #define M(x,y) matrix3x3->value[x][y]
2311 outRationalPoints3D->x = inPoints2D->x*M(0,0) + inPoints2D->y*M(1,0) + M(2,0);
2312 outRationalPoints3D->y = inPoints2D->x*M(0,1) + inPoints2D->y*M(1,1) + M(2,1);
2313 outRationalPoints3D->w = inPoints2D->x*M(0,2) + inPoints2D->y*M(1,2) + M(2,2);
2314 #undef M
2315
2316 ((const char*&) inPoints2D) += inStructSize;
2317 ((char*&) outRationalPoints3D) += outStructSize;
2318 }
2319
2320 return(kQ3Success);
2321 }
2322
2323
2324
2325
2326
2327 //=============================================================================
2328 // E3RationalPoint3D_To3DTransformArray : Transform array of 3D rational
2329 // points by 3x3 matrix.
2330 //-----------------------------------------------------------------------------
2331 // Note : 'outRationalPoints3D' may be the same as 'inRationalPoints3D'.
2332 //-----------------------------------------------------------------------------
2333 TQ3Status
E3RationalPoint3D_To3DTransformArray(const TQ3RationalPoint3D * inRationalPoints3D,const TQ3Matrix3x3 * matrix3x3,TQ3RationalPoint3D * outRationalPoints3D,TQ3Uns32 numPoints,TQ3Uns32 inStructSize,TQ3Uns32 outStructSize)2334 E3RationalPoint3D_To3DTransformArray(const TQ3RationalPoint3D *inRationalPoints3D,
2335 const TQ3Matrix3x3 *matrix3x3,
2336 TQ3RationalPoint3D *outRationalPoints3D,
2337 TQ3Uns32 numPoints,
2338 TQ3Uns32 inStructSize,
2339 TQ3Uns32 outStructSize)
2340 {
2341 TQ3Uns32 i;
2342
2343 for (i = 0; i < numPoints; ++i)
2344 {
2345 E3RationalPoint3D_Transform(inRationalPoints3D, matrix3x3, outRationalPoints3D);
2346
2347 ((const char*&) inRationalPoints3D) += inStructSize;
2348 ((char*&) outRationalPoints3D) += outStructSize;
2349 }
2350
2351 return(kQ3Success);
2352 }
2353
2354
2355
2356
2357
2358 //=============================================================================
2359 // E3Point3D_To3DTransformArray : Transform array of 3D points.
2360 //-----------------------------------------------------------------------------
2361 // Note : 'outPoints3D' may be the same as 'inPoints3D'.
2362 //-----------------------------------------------------------------------------
2363 TQ3Status
E3Point3D_To3DTransformArray(const TQ3Point3D * inPoints3D,const TQ3Matrix4x4 * matrix4x4,TQ3Point3D * outPoints3D,TQ3Uns32 numPoints,TQ3Uns32 inStructSize,TQ3Uns32 outStructSize)2364 E3Point3D_To3DTransformArray(const TQ3Point3D *inPoints3D,
2365 const TQ3Matrix4x4 *matrix4x4,
2366 TQ3Point3D *outPoints3D,
2367 TQ3Uns32 numPoints,
2368 TQ3Uns32 inStructSize,
2369 TQ3Uns32 outStructSize)
2370 {
2371 TQ3Uns32 i;
2372
2373 // Transform the points - will be in-lined in release builds
2374 for (i = 0; i < numPoints; ++i)
2375 {
2376 E3Point3D_Transform(inPoints3D, matrix4x4, outPoints3D);
2377
2378 ((const char*&) inPoints3D) += inStructSize;
2379 ((char*&) outPoints3D) += outStructSize;
2380 }
2381
2382 return(kQ3Success);
2383 }
2384
2385
2386
2387
2388
2389 //=============================================================================
2390 // E3Point3D_To4DTransformArray : Transform array of 3D points by 4x4
2391 // matrix into 4D rational points.
2392 //-----------------------------------------------------------------------------
2393 TQ3Status
E3Point3D_To4DTransformArray(const TQ3Point3D * inPoints3D,const TQ3Matrix4x4 * matrix4x4,TQ3RationalPoint4D * outRationalPoints4D,TQ3Uns32 numPoints,TQ3Uns32 inStructSize,TQ3Uns32 outStructSize)2394 E3Point3D_To4DTransformArray(const TQ3Point3D *inPoints3D,
2395 const TQ3Matrix4x4 *matrix4x4,
2396 TQ3RationalPoint4D *outRationalPoints4D,
2397 TQ3Uns32 numPoints,
2398 TQ3Uns32 inStructSize,
2399 TQ3Uns32 outStructSize)
2400 {
2401 TQ3Uns32 i;
2402
2403 for (i = 0; i < numPoints; ++i)
2404 {
2405 #define M(x,y) matrix4x4->value[x][y]
2406 outRationalPoints4D->x = inPoints3D->x*M(0,0) + inPoints3D->y*M(1,0) + inPoints3D->z*M(2,0) + M(3,0);
2407 outRationalPoints4D->y = inPoints3D->x*M(0,1) + inPoints3D->y*M(1,1) + inPoints3D->z*M(2,1) + M(3,1);
2408 outRationalPoints4D->z = inPoints3D->x*M(0,2) + inPoints3D->y*M(1,2) + inPoints3D->z*M(2,2) + M(3,2);
2409 outRationalPoints4D->w = inPoints3D->x*M(0,3) + inPoints3D->y*M(1,3) + inPoints3D->z*M(2,3) + M(3,3);
2410 #undef M
2411
2412 ((const char*&) inPoints3D) += inStructSize;
2413 ((char*&) outRationalPoints4D) += outStructSize;
2414 }
2415
2416 return(kQ3Success);
2417 }
2418
2419
2420
2421
2422
2423 //=============================================================================
2424 // E3RationalPoint4D_To4DTransformArray : Transform array of 4D rational
2425 // points by 4x4 matrix.
2426 //-----------------------------------------------------------------------------
2427 // Note : 'outRationalPoints4D' may be the same as 'inRationalPoints4D'.
2428 //-----------------------------------------------------------------------------
2429 TQ3Status
E3RationalPoint4D_To4DTransformArray(const TQ3RationalPoint4D * inRationalPoints4D,const TQ3Matrix4x4 * matrix4x4,TQ3RationalPoint4D * outRationalPoints4D,TQ3Uns32 numPoints,TQ3Uns32 inStructSize,TQ3Uns32 outStructSize)2430 E3RationalPoint4D_To4DTransformArray(const TQ3RationalPoint4D *inRationalPoints4D,
2431 const TQ3Matrix4x4 *matrix4x4,
2432 TQ3RationalPoint4D *outRationalPoints4D,
2433 TQ3Uns32 numPoints,
2434 TQ3Uns32 inStructSize,
2435 TQ3Uns32 outStructSize)
2436 {
2437 TQ3Uns32 i;
2438
2439 for (i = 0; i < numPoints; ++i)
2440 {
2441 E3RationalPoint4D_Transform(inRationalPoints4D, matrix4x4, outRationalPoints4D);
2442
2443 ((const char*&) inRationalPoints4D) += inStructSize;
2444 ((char*&) outRationalPoints4D) += outStructSize;
2445 }
2446
2447 return(kQ3Success);
2448 }
2449
2450
2451
2452
2453
2454 //=============================================================================
2455 // E3Matrix3x3_SetIdentity : Set 3x3 identity matrix.
2456 //-----------------------------------------------------------------------------
2457 #pragma mark -
2458 TQ3Matrix3x3 *
E3Matrix3x3_SetIdentity(TQ3Matrix3x3 * matrix3x3)2459 E3Matrix3x3_SetIdentity(TQ3Matrix3x3 *matrix3x3)
2460 {
2461 Q3Memory_Clear(matrix3x3, sizeof(TQ3Matrix3x3));
2462
2463 #define M(x,y) matrix3x3->value[x][y]
2464
2465 M(0,0) = 1.0f;
2466
2467 M(1,1) = 1.0f;
2468
2469 M(2,2) = 1.0f;
2470
2471 #undef M
2472
2473 return(matrix3x3);
2474 }
2475
2476
2477
2478
2479
2480 //=============================================================================
2481 // E3Matrix4x4_SetIdentity : Set 4x4 identity matrix.
2482 //-----------------------------------------------------------------------------
2483 TQ3Matrix4x4 *
E3Matrix4x4_SetIdentity(TQ3Matrix4x4 * matrix4x4)2484 E3Matrix4x4_SetIdentity(TQ3Matrix4x4 *matrix4x4)
2485 {
2486 Q3Memory_Clear(matrix4x4, sizeof(TQ3Matrix4x4));
2487
2488 #define M(x,y) matrix4x4->value[x][y]
2489
2490 M(0,0) = 1.0f;
2491
2492 M(1,1) = 1.0f;
2493
2494 M(2,2) = 1.0f;
2495
2496 M(3,3) = 1.0f;
2497
2498 #undef M
2499
2500 return(matrix4x4);
2501 }
2502
2503
2504
2505
2506
2507 //=============================================================================
2508 // E3Matrix3x3_SetTranslate : Set 3x3 matrix to translate in x, y.
2509 //-----------------------------------------------------------------------------
2510 TQ3Matrix3x3 *
E3Matrix3x3_SetTranslate(TQ3Matrix3x3 * matrix3x3,float xTrans,float yTrans)2511 E3Matrix3x3_SetTranslate(TQ3Matrix3x3 *matrix3x3, float xTrans, float yTrans)
2512 {
2513 Q3Memory_Clear(matrix3x3, sizeof(TQ3Matrix3x3));
2514
2515 #define M(x,y) matrix3x3->value[x][y]
2516
2517 M(0,0) = 1.0f;
2518
2519 M(1,1) = 1.0f;
2520
2521 M(2,0) = xTrans;
2522 M(2,1) = yTrans;
2523 M(2,2) = 1.0f;
2524
2525 #undef M
2526
2527 return(matrix3x3);
2528 }
2529
2530
2531
2532
2533
2534 //=============================================================================
2535 // E3Matrix4x4_SetTranslate : Set 4x4 matrix to translate in x, y, z.
2536 //-----------------------------------------------------------------------------
2537 TQ3Matrix4x4 *
E3Matrix4x4_SetTranslate(TQ3Matrix4x4 * matrix4x4,float xTrans,float yTrans,float zTrans)2538 E3Matrix4x4_SetTranslate(TQ3Matrix4x4 *matrix4x4,
2539 float xTrans, float yTrans, float zTrans)
2540 {
2541 Q3Memory_Clear(matrix4x4, sizeof(TQ3Matrix4x4));
2542
2543 #define M(x,y) matrix4x4->value[x][y]
2544
2545 M(0,0) = 1.0f;
2546
2547 M(1,1) = 1.0f;
2548
2549 M(2,2) = 1.0f;
2550
2551 M(3,0) = xTrans;
2552 M(3,1) = yTrans;
2553 M(3,2) = zTrans;
2554 M(3,3) = 1.0f;
2555
2556 #undef M
2557
2558 return(matrix4x4);
2559 }
2560
2561
2562
2563
2564
2565 //=============================================================================
2566 // E3Matrix3x3_SetScale : Set 3x3 matrix to scale in x, y.
2567 //-----------------------------------------------------------------------------
2568 TQ3Matrix3x3 *
E3Matrix3x3_SetScale(TQ3Matrix3x3 * matrix3x3,float xScale,float yScale)2569 E3Matrix3x3_SetScale(TQ3Matrix3x3 *matrix3x3, float xScale, float yScale)
2570 {
2571 Q3Memory_Clear(matrix3x3, sizeof(TQ3Matrix3x3));
2572
2573 #define M(x,y) matrix3x3->value[x][y]
2574
2575 M(0,0) = xScale;
2576
2577 M(1,1) = yScale;
2578
2579 M(2,2) = 1.0f;
2580
2581 #undef M
2582
2583 return(matrix3x3);
2584 }
2585
2586
2587
2588
2589
2590 //=============================================================================
2591 // E3Matrix4x4_SetScale : Set 4x4 matrix to scale in x, y, z.
2592 //-----------------------------------------------------------------------------
2593 TQ3Matrix4x4 *
E3Matrix4x4_SetScale(TQ3Matrix4x4 * matrix4x4,float xScale,float yScale,float zScale)2594 E3Matrix4x4_SetScale(TQ3Matrix4x4 *matrix4x4,
2595 float xScale, float yScale, float zScale)
2596 {
2597 Q3Memory_Clear(matrix4x4, sizeof(TQ3Matrix4x4));
2598
2599 #define M(x,y) matrix4x4->value[x][y]
2600
2601 M(0,0) = xScale;
2602
2603 M(1,1) = yScale;
2604
2605 M(2,2) = zScale;
2606
2607 M(3,3) = 1.0f;
2608
2609 #undef M
2610
2611 return(matrix4x4);
2612 }
2613
2614
2615
2616
2617
2618 //=============================================================================
2619 // E3Matrix3x3_SetRotate : Set 3x3 matrix to rotate about origin.
2620 //-----------------------------------------------------------------------------
2621 TQ3Matrix3x3 *
E3Matrix3x3_SetRotate(TQ3Matrix3x3 * matrix3x3,float angle)2622 E3Matrix3x3_SetRotate(TQ3Matrix3x3 *matrix3x3, float angle)
2623 {
2624 float cosAngle = (float) cos(angle);
2625 float sinAngle = (float) sin(angle);
2626
2627 Q3Memory_Clear(matrix3x3, sizeof(TQ3Matrix3x3));
2628
2629 #define M(x,y) matrix3x3->value[x][y]
2630
2631 M(0,0) = cosAngle;
2632 M(0,1) = sinAngle;
2633
2634 M(1,0) = -sinAngle;
2635 M(1,1) = cosAngle;
2636
2637 M(2,2) = 1.0f;
2638
2639 #undef M
2640
2641 return(matrix3x3);
2642 }
2643
2644
2645
2646
2647
2648 //=============================================================================
2649 // E3Matrix4x4_SetRotate_X : Set 4x4 matrix to rotate about X axis.
2650 //-----------------------------------------------------------------------------
2651 TQ3Matrix4x4 *
E3Matrix4x4_SetRotate_X(TQ3Matrix4x4 * matrix4x4,float angle)2652 E3Matrix4x4_SetRotate_X(TQ3Matrix4x4 *matrix4x4, float angle)
2653 {
2654 float cosAngle = (float) cos(angle);
2655 float sinAngle = (float) sin(angle);
2656
2657 Q3Memory_Clear(matrix4x4, sizeof(TQ3Matrix4x4));
2658
2659 #define M(x,y) matrix4x4->value[x][y]
2660
2661 M(0,0) = 1.0f;
2662
2663 M(1,1) = cosAngle;
2664 M(1,2) = sinAngle;
2665
2666 M(2,1) = -sinAngle;
2667 M(2,2) = cosAngle;
2668
2669 M(3,3) = 1.0f;
2670
2671 #undef M
2672
2673 return(matrix4x4);
2674 }
2675
2676
2677
2678
2679
2680 //=============================================================================
2681 // E3Matrix4x4_SetRotate_Y : Set 4x4 matrix to rotate about Y axis.
2682 //-----------------------------------------------------------------------------
2683 TQ3Matrix4x4 *
E3Matrix4x4_SetRotate_Y(TQ3Matrix4x4 * matrix4x4,float angle)2684 E3Matrix4x4_SetRotate_Y(TQ3Matrix4x4 *matrix4x4, float angle)
2685 {
2686 float cosAngle = (float) cos(angle);
2687 float sinAngle = (float) sin(angle);
2688
2689 Q3Memory_Clear(matrix4x4, sizeof(TQ3Matrix4x4));
2690
2691 #define M(x,y) matrix4x4->value[x][y]
2692
2693 M(0,0) = cosAngle;
2694 M(0,2) = -sinAngle;
2695
2696 M(1,1) = 1.0f;
2697
2698 M(2,0) = sinAngle;
2699 M(2,2) = cosAngle;
2700
2701 M(3,3) = 1.0f;
2702
2703 #undef M
2704
2705 return(matrix4x4);
2706 }
2707
2708
2709
2710
2711
2712 //=============================================================================
2713 // E3Matrix4x4_SetRotate_Z : Set 4x4 matrix to rotate about Z axis.
2714 //-----------------------------------------------------------------------------
2715 TQ3Matrix4x4 *
E3Matrix4x4_SetRotate_Z(TQ3Matrix4x4 * matrix4x4,float angle)2716 E3Matrix4x4_SetRotate_Z(TQ3Matrix4x4 *matrix4x4, float angle)
2717 {
2718 float cosAngle = (float) cos(angle);
2719 float sinAngle = (float) sin(angle);
2720
2721 Q3Memory_Clear(matrix4x4, sizeof(TQ3Matrix4x4));
2722
2723 #define M(x,y) matrix4x4->value[x][y]
2724
2725 M(0,0) = cosAngle;
2726 M(0,1) = sinAngle;
2727
2728 M(1,0) = -sinAngle;
2729 M(1,1) = cosAngle;
2730
2731 M(2,2) = 1.0f;
2732
2733 M(3,3) = 1.0f;
2734
2735 #undef M
2736
2737 return(matrix4x4);
2738 }
2739
2740
2741
2742
2743
2744 //=============================================================================
2745 // E3Matrix4x4_SetRotate_XYZ : Set 4x4 matrix to rotate about X, Y, Z axes
2746 // (in that order -- which is rarely useful).
2747 //-----------------------------------------------------------------------------
2748 TQ3Matrix4x4 *
E3Matrix4x4_SetRotate_XYZ(TQ3Matrix4x4 * matrix4x4,float xAngle,float yAngle,float zAngle)2749 E3Matrix4x4_SetRotate_XYZ(TQ3Matrix4x4 *matrix4x4,
2750 float xAngle, float yAngle, float zAngle)
2751 {
2752 float cosX = (float) cos(xAngle);
2753 float sinX = (float) sin(xAngle);
2754 float cosY = (float) cos(yAngle);
2755 float sinY = (float) sin(yAngle);
2756 float cosZ = (float) cos(zAngle);
2757 float sinZ = (float) sin(zAngle);
2758
2759 float sinXsinY = sinX*sinY;
2760 float cosXsinY = cosX*sinY;
2761
2762 #define M(x,y) matrix4x4->value[x][y]
2763
2764 M(0,0) = cosY*cosZ;
2765 M(0,1) = cosY*sinZ;
2766 M(0,2) = -sinY;
2767 M(0,3) = 0.0f;
2768
2769 M(1,0) = sinXsinY*cosZ - cosX*sinZ;
2770 M(1,1) = sinXsinY*sinZ + cosX*cosZ;
2771 M(1,2) = sinX*cosY;
2772 M(1,3) = 0.0f;
2773
2774 M(2,0) = cosXsinY*cosZ + sinX*sinZ;
2775 M(2,1) = cosXsinY*sinZ - sinX*cosZ;
2776 M(2,2) = cosX*cosY;
2777 M(2,3) = 0.0f;
2778
2779 M(3,0) = 0.0f;
2780 M(3,1) = 0.0f;
2781 M(3,2) = 0.0f;
2782 M(3,3) = 1.0f;
2783
2784 #undef M
2785
2786 return(matrix4x4);
2787 }
2788
2789
2790
2791
2792
2793 //=============================================================================
2794 // E3Matrix3x3_SetRotateAboutPoint : Set 3x3 matrix to rotate about point.
2795 //-----------------------------------------------------------------------------
2796 TQ3Matrix3x3 *
E3Matrix3x3_SetRotateAboutPoint(TQ3Matrix3x3 * matrix3x3,const TQ3Point2D * origin,float angle)2797 E3Matrix3x3_SetRotateAboutPoint(TQ3Matrix3x3 *matrix3x3,
2798 const TQ3Point2D *origin, float angle)
2799 {
2800 float cosAngle = (float) cos(angle);
2801 float sinAngle = (float) sin(angle);
2802
2803 #define M(x,y) matrix3x3->value[x][y]
2804 #define Dx origin->x
2805 #define Dy origin->y
2806
2807 M(0,0) = cosAngle;
2808 M(0,1) = sinAngle;
2809 M(0,2) = 0.0f;
2810
2811 M(1,0) = -sinAngle;
2812 M(1,1) = cosAngle;
2813 M(1,2) = 0.0f;
2814
2815 M(2,0) = Dx - Dx*cosAngle + Dy*sinAngle; // = Dx - Dx*M(0,0) - Dy*M(1,0)
2816 M(2,1) = Dy - Dx*sinAngle - Dy*cosAngle; // = Dx - Dx*M(0,1) - Dy*M(1,1)
2817 M(2,2) = 1.0f;
2818
2819 #undef M
2820 #undef Dx
2821 #undef Dy
2822
2823 return(matrix3x3);
2824 }
2825
2826
2827
2828
2829
2830 //=============================================================================
2831 // E3Matrix4x4_SetRotateAboutPoint : Set 4x4 matrix to rotate about axes through
2832 // point and parallel to X, Y, Z axes
2833 // (in that order -- which is rarely useful).
2834 //-----------------------------------------------------------------------------
2835 TQ3Matrix4x4 *
E3Matrix4x4_SetRotateAboutPoint(TQ3Matrix4x4 * matrix4x4,const TQ3Point3D * origin,float xAngle,float yAngle,float zAngle)2836 E3Matrix4x4_SetRotateAboutPoint(TQ3Matrix4x4 *matrix4x4,
2837 const TQ3Point3D *origin, float xAngle, float yAngle, float zAngle)
2838 {
2839 float cosX = (float) cos(xAngle);
2840 float sinX = (float) sin(xAngle);
2841 float cosY = (float) cos(yAngle);
2842 float sinY = (float) sin(yAngle);
2843 float cosZ = (float) cos(zAngle);
2844 float sinZ = (float) sin(zAngle);
2845
2846 float sinXsinY = sinX*sinY;
2847 float cosXsinY = cosX*sinY;
2848
2849 #define M(x,y) matrix4x4->value[x][y]
2850 #define Dx origin->x
2851 #define Dy origin->y
2852 #define Dz origin->z
2853
2854 M(0,0) = cosY*cosZ;
2855 M(0,1) = cosY*sinZ;
2856 M(0,2) = -sinY;
2857 M(0,3) = 0.0f;
2858
2859 M(1,0) = sinXsinY*cosZ - cosX*sinZ;
2860 M(1,1) = sinXsinY*sinZ + cosX*cosZ;
2861 M(1,2) = sinX*cosY;
2862 M(1,3) = 0.0f;
2863
2864 M(2,0) = cosXsinY*cosZ + sinX*sinZ;
2865 M(2,1) = cosXsinY*sinZ - sinX*cosZ;
2866 M(2,2) = cosX*cosY;
2867 M(2,3) = 0.0f;
2868
2869 M(3,0) = Dx - Dx*M(0,0) - Dy*M(1,0) - Dz*M(2,0);
2870 M(3,1) = Dy - Dx*M(0,1) - Dy*M(1,1) - Dz*M(2,1);
2871 M(3,2) = Dz - Dx*M(0,2) - Dy*M(1,2) - Dz*M(2,2);
2872 M(3,3) = 1.0f;
2873
2874 #undef M
2875 #undef Dx
2876 #undef Dy
2877 #undef Dz
2878
2879 return(matrix4x4);
2880 }
2881
2882
2883
2884
2885
2886 //=============================================================================
2887 // E3Matrix4x4_SetRotateAboutAxis : Set 4x4 matrix to rotate about
2888 // arbitrary origin and axis.
2889 //-----------------------------------------------------------------------------
2890 // Note : See 'Rotation Tools', pp. 465-469, by Michael E. Pique, and
2891 // 'Matrices and Transformations', pp. 472-475, by Ronald Goldman,
2892 // both in "Graphics Gems".
2893 //
2894 // For correct results, |axis| == 1.
2895 //-----------------------------------------------------------------------------
2896 TQ3Matrix4x4 *
E3Matrix4x4_SetRotateAboutAxis(TQ3Matrix4x4 * matrix4x4,const TQ3Point3D * origin,const TQ3Vector3D * axis,float angle)2897 E3Matrix4x4_SetRotateAboutAxis(TQ3Matrix4x4 *matrix4x4,
2898 const TQ3Point3D *origin, const TQ3Vector3D *axis, float angle)
2899 {
2900 float c = (float) cos(angle);
2901 float s = (float) sin(angle);
2902 float t = 1 - c;
2903 float tx = t*axis->x;
2904 float ty = t*axis->y;
2905 float tz = t*axis->z;
2906 float sx = s*axis->x;
2907 float sy = s*axis->y;
2908 float sz = s*axis->z;
2909
2910 #define M(x,y) matrix4x4->value[x][y]
2911 #define Dx origin->x
2912 #define Dy origin->y
2913 #define Dz origin->z
2914
2915 M(0,0) = tx*axis->x + c;
2916 M(0,1) = tx*axis->y + sz;
2917 M(0,2) = tx*axis->z - sy;
2918 M(0,3) = 0.0f;
2919
2920 M(1,0) = ty*axis->x - sz;
2921 M(1,1) = ty*axis->y + c;
2922 M(1,2) = ty*axis->z + sx;
2923 M(1,3) = 0.0f;
2924
2925 M(2,0) = tz*axis->x + sy;
2926 M(2,1) = tz*axis->y - sx;
2927 M(2,2) = tz*axis->z + c;
2928 M(2,3) = 0.0f;
2929
2930 M(3,0) = Dx - Dx*M(0,0) - Dy*M(1,0) - Dz*M(2,0);
2931 M(3,1) = Dy - Dx*M(0,1) - Dy*M(1,1) - Dz*M(2,1);
2932 M(3,2) = Dz - Dx*M(0,2) - Dy*M(1,2) - Dz*M(2,2);
2933 M(3,3) = 1.0f;
2934
2935 #undef M
2936 #undef Dx
2937 #undef Dy
2938 #undef Dz
2939
2940 return(matrix4x4);
2941 }
2942
2943
2944
2945
2946
2947 //=============================================================================
2948 // E3Matrix4x4_SetRotateVectorToVector : Set 4x4 matrix to rotate 'v1'
2949 // to 'v2'.
2950 //-----------------------------------------------------------------------------
2951 // Note : See Foley, et al., 2nd ed., pp. 220-222.
2952 //
2953 // For correct results, |v1| == 1 && |v2| == 1.
2954 //-----------------------------------------------------------------------------
2955 TQ3Matrix4x4 *
E3Matrix4x4_SetRotateVectorToVector(TQ3Matrix4x4 * matrix4x4,const TQ3Vector3D * v1,const TQ3Vector3D * v2)2956 E3Matrix4x4_SetRotateVectorToVector(TQ3Matrix4x4 *matrix4x4,
2957 const TQ3Vector3D *v1, const TQ3Vector3D *v2)
2958 {
2959 // We accomplish the rotation by creating two orthonormal vector triads:
2960 //
2961 // (u, v, w) -> (u', v', w)
2962 //
2963 // The rotation is about the axis w. It rotates u (=v1) into u' (=v2). It
2964 // also rotates v into v', which are vectors rotated 90 degrees in the plane
2965 // of rotation from u and u', respectively.
2966 //
2967 // To construct the rotation matrix, we rotate into and out of the basis
2968 // vectors i, j, k:
2969 //
2970 // (u, v, w) -> (i, j, k) -> (u', v', w)
2971 //
2972 // Thus the rotation matrix is the product of two rotation matrices, a and b:
2973 //
2974 // | ux vx wx | | ux' uy' uz' |
2975 // | uy vy wy | . | vx' vy' vz' |
2976 // | uz vz wz | | wx wy wz |
2977 //
2978 // To see this, simply multiply this composite matrix by the u, v or w row
2979 // vector on the left and see that the result is u', v' or w, respectively.
2980
2981 TQ3Vector3D u, uPrime, v, vPrime, w;
2982 TQ3Matrix3x3 a, b;
2983
2984 // Construct vector w (axis of rotation) perpendicular to v1 and v2
2985 Q3Vector3D_Cross(v1, v2, &w);
2986
2987 // Check if vector w is roughly zero
2988 if (e3vector3d_below_tolerance(&w, 10.0f*FLT_EPSILON))
2989 {
2990 // Vectors v1 and v2 are approximately parallel or approximately anti-parallel
2991 // (within 1.192092896e-07 radians or roughly 0.000007 degrees!)
2992
2993 TQ3Vector3D v2Proxy;
2994 TQ3Int32 iSmall, i;
2995 float valueSmall;
2996
2997 // Determine v1 component of smallest absolute value
2998 iSmall = 0;
2999 valueSmall = (float) fabs(v1->x);
3000 for (i = 1; i < 3; ++i)
3001 {
3002 float value;
3003
3004 value = (float) fabs(((const float*) (v1))[i]);
3005 if (value < valueSmall)
3006 {
3007 iSmall = i;
3008 valueSmall = value;
3009 }
3010 }
3011
3012 // Construct corresponding basis vector
3013 for (i = 0; i < 3; ++i)
3014 ((float*) (&v2Proxy))[i] = (i == iSmall ? 1.0f : 0.0f);
3015
3016 // Construct vector w (axis of rotation) perpendicular to v1 and v2Proxy
3017 Q3Vector3D_Cross(v1, &v2Proxy, &w);
3018 }
3019
3020 Q3Vector3D_Normalize(&w, &w);
3021
3022 u = *v1;
3023 uPrime = *v2;
3024 Q3Vector3D_Cross(&w, &u, &v);
3025 Q3Vector3D_Cross(&w, &uPrime, &vPrime);
3026
3027 #define A(x,y) a.value[x][y]
3028 #define B(x,y) b.value[x][y]
3029 #define M(x,y) matrix4x4->value[x][y]
3030
3031 A(0,0) = u.x;
3032 A(0,1) = v.x;
3033 A(0,2) = w.x;
3034
3035 A(1,0) = u.y;
3036 A(1,1) = v.y;
3037 A(1,2) = w.y;
3038
3039 A(2,0) = u.z;
3040 A(2,1) = v.z;
3041 A(2,2) = w.z;
3042
3043 B(0,0) = uPrime.x;
3044 B(0,1) = uPrime.y;
3045 B(0,2) = uPrime.z;
3046
3047 B(1,0) = vPrime.x;
3048 B(1,1) = vPrime.y;
3049 B(1,2) = vPrime.z;
3050
3051 B(2,0) = w.x;
3052 B(2,1) = w.y;
3053 B(2,2) = w.z;
3054
3055 // Now multiply the two 3x3 matrices a and b to get the 3x3 part of the result,
3056 // filling out the rest of the result as an identity matrix (since we are
3057 // rotating about the point <0,0,0>)
3058 M(0,0) = A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0);
3059 M(0,1) = A(0,0)*B(0,1) + A(0,1)*B(1,1) + A(0,2)*B(2,1);
3060 M(0,2) = A(0,0)*B(0,2) + A(0,1)*B(1,2) + A(0,2)*B(2,2);
3061 M(0,3) = 0.0f;
3062
3063 M(1,0) = A(1,0)*B(0,0) + A(1,1)*B(1,0) + A(1,2)*B(2,0);
3064 M(1,1) = A(1,0)*B(0,1) + A(1,1)*B(1,1) + A(1,2)*B(2,1);
3065 M(1,2) = A(1,0)*B(0,2) + A(1,1)*B(1,2) + A(1,2)*B(2,2);
3066 M(1,3) = 0.0f;
3067
3068 M(2,0) = A(2,0)*B(0,0) + A(2,1)*B(1,0) + A(2,2)*B(2,0);
3069 M(2,1) = A(2,0)*B(0,1) + A(2,1)*B(1,1) + A(2,2)*B(2,1);
3070 M(2,2) = A(2,0)*B(0,2) + A(2,1)*B(1,2) + A(2,2)*B(2,2);
3071 M(2,3) = 0.0f;
3072
3073 M(3,0) = 0.0f;
3074 M(3,1) = 0.0f;
3075 M(3,2) = 0.0f;
3076 M(3,3) = 1.0f;
3077
3078 #undef A
3079 #undef B
3080 #undef M
3081
3082 return(matrix4x4);
3083 }
3084
3085
3086
3087
3088
3089 //=============================================================================
3090 // E3Matrix4x4_SetQuaternion : Set 4x4 matrix from quaternion.
3091 //-----------------------------------------------------------------------------
3092 // Note : See http://www.gamasutra.com/features/19980703/quaternions_01.htm
3093 // Since Quesa uses row vectors rather than the more conventional
3094 // column vectors, we multiply in the opposite order: row*matrix rather
3095 // matrix*column. Thus our matrices are the transpose of theirs.
3096 //-----------------------------------------------------------------------------
3097 TQ3Matrix4x4 *
E3Matrix4x4_SetQuaternion(TQ3Matrix4x4 * matrix4x4,const TQ3Quaternion * quaternion)3098 E3Matrix4x4_SetQuaternion(TQ3Matrix4x4 *matrix4x4, const TQ3Quaternion *quaternion)
3099 {
3100 // calculate coefficients
3101 float x2 = quaternion->x + quaternion->x;
3102 float y2 = quaternion->y + quaternion->y;
3103 float z2 = quaternion->z + quaternion->z;
3104 float xx = quaternion->x * x2;
3105 float xy = quaternion->x * y2;
3106 float xz = quaternion->x * z2;
3107 float yy = quaternion->y * y2;
3108 float yz = quaternion->y * z2;
3109 float zz = quaternion->z * z2;
3110 float wx = quaternion->w * x2;
3111 float wy = quaternion->w * y2;
3112 float wz = quaternion->w * z2;
3113
3114 #define M(x,y) matrix4x4->value[x][y]
3115
3116 M(0,0) = 1.0f - (yy + zz);
3117 M(0,1) = xy + wz;
3118 M(0,2) = xz - wy;
3119 M(0,3) = 0.0f;
3120
3121 M(1,0) = xy - wz;
3122 M(1,1) = 1.0f - (xx + zz);
3123 M(1,2) = yz + wx;
3124 M(1,3) = 0.0f;
3125
3126 M(2,0) = xz + wy;
3127 M(2,1) = yz - wx;
3128 M(2,2) = 1.0f - (xx + yy);
3129 M(2,3) = 0.0f;
3130
3131 M(3,0) = 0.0f;
3132 M(3,1) = 0.0f;
3133 M(3,2) = 0.0f;
3134 M(3,3) = 1.0f;
3135
3136 #undef M
3137
3138 return(matrix4x4);
3139 }
3140
3141
3142
3143
3144
3145 //=============================================================================
3146 // E3Matrix3x3_Copy : Copy 3x3 matrix.
3147 //-----------------------------------------------------------------------------
3148 // Note : 'result' may be the same as 'matrix3x3'.
3149 //-----------------------------------------------------------------------------
3150 TQ3Matrix3x3 *
E3Matrix3x3_Copy(const TQ3Matrix3x3 * matrix3x3,TQ3Matrix3x3 * result)3151 E3Matrix3x3_Copy(const TQ3Matrix3x3 *matrix3x3, TQ3Matrix3x3 *result)
3152 {
3153 if (result != matrix3x3)
3154 *result = *matrix3x3;
3155
3156 return(result);
3157 }
3158
3159
3160
3161
3162
3163 //=============================================================================
3164 // E3Matrix4x4_Copy : Copy 4x4 matrix.
3165 //-----------------------------------------------------------------------------
3166 // Note : 'result' may be the same as 'matrix4x4'.
3167 //-----------------------------------------------------------------------------
3168 TQ3Matrix4x4 *
E3Matrix4x4_Copy(const TQ3Matrix4x4 * matrix4x4,TQ3Matrix4x4 * result)3169 E3Matrix4x4_Copy(const TQ3Matrix4x4 *matrix4x4, TQ3Matrix4x4 *result)
3170 {
3171 if (result != matrix4x4)
3172 *result = *matrix4x4;
3173
3174 return(result);
3175 }
3176
3177
3178
3179
3180
3181 //=============================================================================
3182 // E3Matrix3x3_Transpose : Transpose 3x3 matrix.
3183 //-----------------------------------------------------------------------------
3184 // Note : 'result' may be the same 'matrix3x3'.
3185 //-----------------------------------------------------------------------------
3186 TQ3Matrix3x3 *
E3Matrix3x3_Transpose(const TQ3Matrix3x3 * matrix3x3,TQ3Matrix3x3 * result)3187 E3Matrix3x3_Transpose(const TQ3Matrix3x3 *matrix3x3, TQ3Matrix3x3 *result)
3188 {
3189 TQ3Int32 i, j;
3190
3191 if (result != matrix3x3)
3192 {
3193 for (i = 0; i < 3; ++i)
3194 for (j = 0; j < 3; ++j)
3195 result->value[i][j] = matrix3x3->value[j][i];
3196 }
3197 else
3198 {
3199 E3Float_Swap(result->value[1][0], result->value[0][1]);
3200 E3Float_Swap(result->value[2][0], result->value[0][2]);
3201 E3Float_Swap(result->value[1][2], result->value[2][1]);
3202 }
3203 return(result);
3204 }
3205
3206
3207
3208
3209
3210 //=============================================================================
3211 // E3Matrix4x4_Transpose : Transpose 4x4 matrix.
3212 //-----------------------------------------------------------------------------
3213 // Note : 'result' may be the same 'matrix4x4'.
3214 //-----------------------------------------------------------------------------
3215 TQ3Matrix4x4 *
E3Matrix4x4_Transpose(const TQ3Matrix4x4 * matrix4x4,TQ3Matrix4x4 * result)3216 E3Matrix4x4_Transpose(const TQ3Matrix4x4 *matrix4x4, TQ3Matrix4x4 *result)
3217 {
3218 TQ3Int32 i, j;
3219
3220 if (result != matrix4x4)
3221 {
3222 for (i = 0; i < 4; ++i)
3223 for (j = 0; j < 4; ++j)
3224 result->value[i][j] = matrix4x4->value[j][i];
3225 }
3226 else
3227 {
3228 E3Float_Swap(result->value[1][0], result->value[0][1]);
3229 E3Float_Swap(result->value[2][0], result->value[0][2]);
3230 E3Float_Swap(result->value[3][0], result->value[0][3]);
3231 E3Float_Swap(result->value[2][1], result->value[1][2]);
3232 E3Float_Swap(result->value[3][1], result->value[1][3]);
3233 E3Float_Swap(result->value[2][3], result->value[3][2]);
3234 }
3235 return(result);
3236 }
3237
3238
3239
3240
3241
3242 //=============================================================================
3243 // E3Matrix3x3_Determinant : Return determinant of 3x3 matrix.
3244 //-----------------------------------------------------------------------------
3245 float
E3Matrix3x3_Determinant(const TQ3Matrix3x3 * matrix3x3)3246 E3Matrix3x3_Determinant(const TQ3Matrix3x3 *matrix3x3)
3247 {
3248 TQ3Matrix3x3 temp = *matrix3x3;
3249
3250 return(e3matrix3x3_determinant(&temp));
3251 }
3252
3253
3254
3255
3256
3257 //=============================================================================
3258 // E3Matrix4x4_Determinant : Return determinant of 4x4 matrix.
3259 //-----------------------------------------------------------------------------
3260 float
E3Matrix4x4_Determinant(const TQ3Matrix4x4 * matrix4x4)3261 E3Matrix4x4_Determinant(const TQ3Matrix4x4 *matrix4x4)
3262 {
3263 TQ3Matrix4x4 temp = *matrix4x4;
3264
3265 return(e3matrix4x4_determinant(&temp));
3266 }
3267
3268
3269
3270
3271
3272 //=============================================================================
3273 // E3Matrix3x3_Adjoint : Calculate adjoint of 3x3 matrix.
3274 //-----------------------------------------------------------------------------
3275 // Note : 'result' may be the same 'matrix3x3'.
3276 //
3277 // The adjoint of a matrix is a scalar multiple of the inverse of
3278 // the matrix. For some applications, the adjoint can be used in
3279 // place of the inverse. In particular:
3280 //
3281 // adjoint(A) = determinant(A) * inverse(A)
3282 //-----------------------------------------------------------------------------
3283 TQ3Matrix3x3 *
E3Matrix3x3_Adjoint(const TQ3Matrix3x3 * matrix3x3,TQ3Matrix3x3 * result)3284 E3Matrix3x3_Adjoint(const TQ3Matrix3x3 *matrix3x3, TQ3Matrix3x3 *result)
3285 {
3286 // If result is alias of input, output to temporary
3287 TQ3Matrix3x3 temp;
3288 TQ3Matrix3x3* output = (result == matrix3x3 ? &temp : result);
3289
3290 #define A(x,y) matrix3x3->value[x][y]
3291 #define B(x,y) output->value[y][x]
3292
3293 // B(x,y) is set up to do the transpose for us!
3294
3295 B(0,0) = (A(1,1)*A(2,2)-A(2,1)*A(1,2));
3296 B(0,1) = -(A(1,0)*A(2,2)-A(2,0)*A(1,2));
3297 B(0,2) = (A(1,0)*A(2,1)-A(2,0)*A(1,1));
3298
3299 B(1,0) = -(A(0,1)*A(2,2)-A(2,1)*A(0,2));
3300 B(1,1) = (A(0,0)*A(2,2)-A(2,0)*A(0,2));
3301 B(1,2) = -(A(0,0)*A(2,1)-A(2,0)*A(0,1));
3302
3303 B(2,0) = (A(0,1)*A(1,2)-A(1,1)*A(0,2));
3304 B(2,1) = -(A(0,0)*A(1,2)-A(1,0)*A(0,2));
3305 B(2,2) = (A(0,0)*A(1,1)-A(1,0)*A(0,1));
3306
3307 #undef A
3308 #undef B
3309
3310 if (output == &temp)
3311 *result = temp;
3312
3313 return(result);
3314 }
3315
3316
3317
3318
3319
3320 //=============================================================================
3321 // E3Matrix3x3_Invert : Invert 3x3 non-singular matrix.
3322 //-----------------------------------------------------------------------------
3323 // Note : 'result' may be the same 'matrix3x3'.
3324 //-----------------------------------------------------------------------------
3325 TQ3Matrix3x3 *
E3Matrix3x3_Invert(const TQ3Matrix3x3 * matrix3x3,TQ3Matrix3x3 * result)3326 E3Matrix3x3_Invert(const TQ3Matrix3x3 *matrix3x3, TQ3Matrix3x3 *result)
3327 {
3328 if (result != matrix3x3)
3329 *result = *matrix3x3;
3330
3331 e3matrix3x3_invert(result);
3332
3333 return(result);
3334 }
3335
3336
3337
3338
3339
3340 //=============================================================================
3341 // E3Matrix4x4_Invert : Invert 4x4 non-singular matrix.
3342 //-----------------------------------------------------------------------------
3343 // Note : 'result' may be the same 'matrix4x4'.
3344 //-----------------------------------------------------------------------------
3345 TQ3Matrix4x4 *
E3Matrix4x4_Invert(const TQ3Matrix4x4 * matrix4x4,TQ3Matrix4x4 * result)3346 E3Matrix4x4_Invert(const TQ3Matrix4x4 *matrix4x4, TQ3Matrix4x4 *result)
3347 {
3348 if (result != matrix4x4)
3349 *result = *matrix4x4;
3350
3351 e3matrix4x4_invert(result);
3352
3353 return(result);
3354 }
3355
3356
3357
3358
3359
3360 //=============================================================================
3361 // E3Matrix3x3_Multiply : Multiply two 3x3 matrices.
3362 //-----------------------------------------------------------------------------
3363 // Note : 'result' may be the same as 'm1' and/or 'm2'.
3364 //-----------------------------------------------------------------------------
3365 TQ3Matrix3x3 *
E3Matrix3x3_Multiply(const TQ3Matrix3x3 * m1,const TQ3Matrix3x3 * m2,TQ3Matrix3x3 * result)3366 E3Matrix3x3_Multiply(const TQ3Matrix3x3 *m1, const TQ3Matrix3x3 *m2, TQ3Matrix3x3 *result)
3367 {
3368 // If result is alias of input, output to temporary
3369 TQ3Matrix3x3 temp;
3370 TQ3Matrix3x3* output = (result == m1 || result == m2 ? &temp : result);
3371
3372 #define A(x,y) m1->value[x][y]
3373 #define B(x,y) m2->value[x][y]
3374 #define M(x,y) output->value[x][y]
3375
3376 M(0,0) = A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0);
3377 M(0,1) = A(0,0)*B(0,1) + A(0,1)*B(1,1) + A(0,2)*B(2,1);
3378 M(0,2) = A(0,0)*B(0,2) + A(0,1)*B(1,2) + A(0,2)*B(2,2);
3379
3380 M(1,0) = A(1,0)*B(0,0) + A(1,1)*B(1,0) + A(1,2)*B(2,0);
3381 M(1,1) = A(1,0)*B(0,1) + A(1,1)*B(1,1) + A(1,2)*B(2,1);
3382 M(1,2) = A(1,0)*B(0,2) + A(1,1)*B(1,2) + A(1,2)*B(2,2);
3383
3384 M(2,0) = A(2,0)*B(0,0) + A(2,1)*B(1,0) + A(2,2)*B(2,0);
3385 M(2,1) = A(2,0)*B(0,1) + A(2,1)*B(1,1) + A(2,2)*B(2,1);
3386 M(2,2) = A(2,0)*B(0,2) + A(2,1)*B(1,2) + A(2,2)*B(2,2);
3387
3388 #undef A
3389 #undef B
3390 #undef M
3391
3392 if (output == &temp)
3393 *result = temp;
3394
3395 return(result);
3396 }
3397
3398
3399
3400
3401
3402 //=============================================================================
3403 // E3Matrix4x4_Multiply : Multiply two 4x4 matrices.
3404 //-----------------------------------------------------------------------------
3405 // Note : 'result' may be the same as 'm1' and/or 'm2'.
3406 //-----------------------------------------------------------------------------
3407 TQ3Matrix4x4 *
E3Matrix4x4_Multiply(const TQ3Matrix4x4 * m1,const TQ3Matrix4x4 * m2,TQ3Matrix4x4 * result)3408 E3Matrix4x4_Multiply(const TQ3Matrix4x4 *m1, const TQ3Matrix4x4 *m2, TQ3Matrix4x4 *result)
3409 {
3410 // If result is alias of input, output to temporary
3411 TQ3Matrix4x4 temp;
3412 TQ3Matrix4x4* output = (result == m1 || result == m2 ? &temp : result);
3413
3414 #define A(x,y) m1->value[x][y]
3415 #define B(x,y) m2->value[x][y]
3416 #define M(x,y) output->value[x][y]
3417
3418 M(0,0) = A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) + A(0,3)*B(3,0);
3419 M(0,1) = A(0,0)*B(0,1) + A(0,1)*B(1,1) + A(0,2)*B(2,1) + A(0,3)*B(3,1);
3420 M(0,2) = A(0,0)*B(0,2) + A(0,1)*B(1,2) + A(0,2)*B(2,2) + A(0,3)*B(3,2);
3421 M(0,3) = A(0,0)*B(0,3) + A(0,1)*B(1,3) + A(0,2)*B(2,3) + A(0,3)*B(3,3);
3422
3423 M(1,0) = A(1,0)*B(0,0) + A(1,1)*B(1,0) + A(1,2)*B(2,0) + A(1,3)*B(3,0);
3424 M(1,1) = A(1,0)*B(0,1) + A(1,1)*B(1,1) + A(1,2)*B(2,1) + A(1,3)*B(3,1);
3425 M(1,2) = A(1,0)*B(0,2) + A(1,1)*B(1,2) + A(1,2)*B(2,2) + A(1,3)*B(3,2);
3426 M(1,3) = A(1,0)*B(0,3) + A(1,1)*B(1,3) + A(1,2)*B(2,3) + A(1,3)*B(3,3);
3427
3428 M(2,0) = A(2,0)*B(0,0) + A(2,1)*B(1,0) + A(2,2)*B(2,0) + A(2,3)*B(3,0);
3429 M(2,1) = A(2,0)*B(0,1) + A(2,1)*B(1,1) + A(2,2)*B(2,1) + A(2,3)*B(3,1);
3430 M(2,2) = A(2,0)*B(0,2) + A(2,1)*B(1,2) + A(2,2)*B(2,2) + A(2,3)*B(3,2);
3431 M(2,3) = A(2,0)*B(0,3) + A(2,1)*B(1,3) + A(2,2)*B(2,3) + A(2,3)*B(3,3);
3432
3433 M(3,0) = A(3,0)*B(0,0) + A(3,1)*B(1,0) + A(3,2)*B(2,0) + A(3,3)*B(3,0);
3434 M(3,1) = A(3,0)*B(0,1) + A(3,1)*B(1,1) + A(3,2)*B(2,1) + A(3,3)*B(3,1);
3435 M(3,2) = A(3,0)*B(0,2) + A(3,1)*B(1,2) + A(3,2)*B(2,2) + A(3,3)*B(3,2);
3436 M(3,3) = A(3,0)*B(0,3) + A(3,1)*B(1,3) + A(3,2)*B(2,3) + A(3,3)*B(3,3);
3437
3438 #undef A
3439 #undef B
3440 #undef M
3441
3442 if (output == &temp)
3443 *result = temp;
3444
3445 return(result);
3446 }
3447
3448
3449
3450
3451
3452 //=============================================================================
3453 // E3Quaternion_Set : Set quaternion.
3454 //-----------------------------------------------------------------------------
3455 #pragma mark -
3456 TQ3Quaternion *
E3Quaternion_Set(TQ3Quaternion * quaternion,float w,float x,float y,float z)3457 E3Quaternion_Set(TQ3Quaternion *quaternion, float w, float x, float y, float z)
3458 {
3459 Q3FastQuaternion_Set(quaternion, w, x, y, z);
3460 return(quaternion);
3461 }
3462
3463
3464
3465
3466
3467 //=============================================================================
3468 // E3Quaternion_SetIdentity : Set quaternion to (1,0,0,0).
3469 //-----------------------------------------------------------------------------
3470 TQ3Quaternion *
E3Quaternion_SetIdentity(TQ3Quaternion * quaternion)3471 E3Quaternion_SetIdentity(TQ3Quaternion *quaternion)
3472 {
3473 Q3FastQuaternion_SetIdentity(quaternion);
3474 return(quaternion);
3475 }
3476
3477
3478
3479
3480
3481 //=============================================================================
3482 // E3Quaternion_SetRotate_X : Set quaternion to rotate about X axis.
3483 //-----------------------------------------------------------------------------
3484 TQ3Quaternion *
E3Quaternion_SetRotate_X(TQ3Quaternion * quaternion,float angle)3485 E3Quaternion_SetRotate_X(TQ3Quaternion *quaternion, float angle)
3486 {
3487 float halfAngle = 0.5f*angle;
3488
3489 quaternion->w = (float) cos(halfAngle);
3490 quaternion->x = (float) sin(halfAngle);
3491 quaternion->y = 0.0f;
3492 quaternion->z = 0.0f;
3493
3494 return(quaternion);
3495 }
3496
3497
3498
3499
3500
3501 //=============================================================================
3502 // E3Quaternion_SetRotate_Y : Set quaternion to rotate about Y axis.
3503 //-----------------------------------------------------------------------------
3504 TQ3Quaternion *
E3Quaternion_SetRotate_Y(TQ3Quaternion * quaternion,float angle)3505 E3Quaternion_SetRotate_Y(TQ3Quaternion *quaternion, float angle)
3506 {
3507 float halfAngle = 0.5f*angle;
3508
3509 quaternion->w = (float) cos(halfAngle);
3510 quaternion->x = 0.0f;
3511 quaternion->y = (float) sin(halfAngle);
3512 quaternion->z = 0.0f;
3513
3514 return(quaternion);
3515 }
3516
3517
3518
3519
3520
3521 //=============================================================================
3522 // E3Quaternion_SetRotate_Z : Set quaternion to rotate about Z axis.
3523 //-----------------------------------------------------------------------------
3524 TQ3Quaternion *
E3Quaternion_SetRotate_Z(TQ3Quaternion * quaternion,float angle)3525 E3Quaternion_SetRotate_Z(TQ3Quaternion *quaternion, float angle)
3526 {
3527 float halfAngle = 0.5f*angle;
3528
3529 quaternion->w = (float) cos(halfAngle);
3530 quaternion->x = 0.0f;
3531 quaternion->y = 0.0f;
3532 quaternion->z = (float) sin(halfAngle);
3533
3534 return(quaternion);
3535 }
3536
3537
3538
3539
3540
3541 //=============================================================================
3542 // E3Quaternion_SetRotate_XYZ : Set quaternion to rotate about X, Y, Z
3543 // axes (in that order -- which is rarely
3544 // useful).
3545 //-----------------------------------------------------------------------------
3546 TQ3Quaternion *
E3Quaternion_SetRotate_XYZ(TQ3Quaternion * quaternion,float xAngle,float yAngle,float zAngle)3547 E3Quaternion_SetRotate_XYZ(TQ3Quaternion *quaternion,
3548 float xAngle, float yAngle, float zAngle)
3549 {
3550 float xHalfAngle = 0.5f*xAngle;
3551 float yHalfAngle = 0.5f*yAngle;
3552 float zHalfAngle = 0.5f*zAngle;
3553
3554 float cosHalfX = (float) cos(xHalfAngle);
3555 float sinHalfX = (float) sin(xHalfAngle);
3556 float cosHalfY = (float) cos(yHalfAngle);
3557 float sinHalfY = (float) sin(yHalfAngle);
3558 float cosHalfZ = (float) cos(zHalfAngle);
3559 float sinHalfZ = (float) sin(zHalfAngle);
3560
3561 float cosHalfYcosHalfZ = cosHalfY*cosHalfZ;
3562 float sinHalfYsinHalfZ = sinHalfY*sinHalfZ;
3563
3564 quaternion->w = cosHalfX*cosHalfYcosHalfZ + sinHalfX*sinHalfYsinHalfZ;
3565 quaternion->x = sinHalfX*cosHalfYcosHalfZ - cosHalfX*sinHalfYsinHalfZ;
3566 quaternion->y = cosHalfX*sinHalfY*cosHalfZ + sinHalfX*cosHalfY*sinHalfZ;
3567 quaternion->z = cosHalfX*cosHalfY*sinHalfZ - sinHalfX*sinHalfY*cosHalfZ;
3568
3569 return(quaternion);
3570 }
3571
3572
3573
3574
3575
3576 //=============================================================================
3577 // E3Quaternion_SetRotateAboutAxis : Set quaternion to rotate about
3578 // arbitrary axis.
3579 //-----------------------------------------------------------------------------
3580 // Note : For correct results, |axis| == 1.
3581 //-----------------------------------------------------------------------------
3582 TQ3Quaternion *
E3Quaternion_SetRotateAboutAxis(TQ3Quaternion * quaternion,const TQ3Vector3D * axis,float angle)3583 E3Quaternion_SetRotateAboutAxis(TQ3Quaternion *quaternion,
3584 const TQ3Vector3D *axis, float angle)
3585 {
3586 float halfAngle = 0.5f*angle;
3587 float cosHalfAngle = (float) cos(halfAngle);
3588 float sinHalfAngle = (float) sin(halfAngle);
3589
3590 quaternion->w = cosHalfAngle;
3591 quaternion->x = axis->x * sinHalfAngle;
3592 quaternion->y = axis->y * sinHalfAngle;
3593 quaternion->z = axis->z * sinHalfAngle;
3594
3595 // ahhh... this is so much easier than matrices...
3596 return(quaternion);
3597 }
3598
3599
3600
3601
3602
3603 //=============================================================================
3604 // E3Quaternion_SetRotateVectorToVector : Set quaternion to rotate 'v1'
3605 // to 'v2'.
3606 //-----------------------------------------------------------------------------
3607 // Note : For correct results, |v1| == 1 && |v2| == 1.
3608 //-----------------------------------------------------------------------------
3609 TQ3Quaternion *
E3Quaternion_SetRotateVectorToVector(TQ3Quaternion * quaternion,const TQ3Vector3D * v1,const TQ3Vector3D * v2)3610 E3Quaternion_SetRotateVectorToVector(TQ3Quaternion *quaternion,
3611 const TQ3Vector3D *v1, const TQ3Vector3D *v2)
3612 {
3613 float cosAngle;
3614 TQ3Vector3D axis;
3615
3616 cosAngle = Q3Vector3D_Dot(v1, v2);
3617 Q3Vector3D_Cross(v1, v2, &axis);
3618
3619 // Note: sin(angle) = |axis|
3620
3621 if (!e3vector3d_below_tolerance(&axis, 10.0f*FLT_EPSILON))
3622 {
3623 // Vectors are neither approximately parallel nor approximately anti-parallel
3624
3625 float cosHalfAngle;
3626 float factor;
3627
3628 cosHalfAngle = E3Math_SquareRoot((1.0f + cosAngle) * 0.5f);
3629
3630 // Note: sin(angle/2) = sin(angle) / (2 * cos(angle/2)) = sin(angle) * factor
3631
3632 factor = 1.0f / (2.0f * cosHalfAngle);
3633
3634 quaternion->w = cosHalfAngle;
3635 quaternion->x = axis.x * factor;
3636 quaternion->y = axis.y * factor;
3637 quaternion->z = axis.z * factor;
3638 }
3639 else
3640 {
3641 // Vectors are approximately parallel or approximately anti-parallel
3642
3643 // cos(angle) is approximately +1 or approximately -1
3644
3645 if (cosAngle < 0.0f)
3646 {
3647 // Vectors are approximately anti-parallel
3648
3649 TQ3Vector3D v2Prime;
3650 TQ3Int32 iSmall, i;
3651 float valueSmall;
3652 float factor;
3653
3654 // Determine v1 component of smallest in absolute value
3655 iSmall = 0;
3656 valueSmall = (float) fabs (v1->x);
3657 for (i = 1; i < 3; ++i)
3658 {
3659 float value;
3660
3661 value = (float) fabs(((const float*) v1)[i]);
3662 if (value < valueSmall)
3663 {
3664 iSmall = i;
3665 valueSmall = value;
3666 }
3667 }
3668
3669 // Construct corresponding basis vector
3670 for (i = 0; i < 3; ++i)
3671 ((float*) &v2Prime)[i] = (i == iSmall ? 1.0f : 0.0f);
3672
3673 // Axis is vector perpendicular to v1 and v2Prime
3674 Q3Vector3D_Cross(v1, &v2Prime, &axis);
3675
3676 // Note: 1 = sin(angle/2) = |axis| * factor
3677
3678 factor = 1.0f / Q3Vector3D_Length(&axis);
3679
3680 quaternion->w = 0.0f;
3681 quaternion->x = axis.x * factor;
3682 quaternion->y = axis.y * factor;
3683 quaternion->z = axis.z * factor;
3684 }
3685 else
3686 {
3687 // Vectors are approximately parallel
3688
3689 quaternion->w = 1.0f;
3690 quaternion->x = 0.0f;
3691 quaternion->y = 0.0f;
3692 quaternion->z = 0.0f;
3693 }
3694 }
3695
3696 return(quaternion);
3697 }
3698
3699
3700
3701
3702
3703 //=============================================================================
3704 // E3Quaternion_SetMatrix : Set quaternion from rotation matrix.
3705 //-----------------------------------------------------------------------------
3706 // Note : The QD3D implementation of this function appears to be buggy.
3707 //
3708 // You can demonstrate this by starting with an arbitrary
3709 // quaternion, converting to a matrix, then converting back (with
3710 // this function).
3711 //
3712 // QD3D's result is something ridiculous; this function returns
3713 // the original quaternion (or something equivalent). I don't know
3714 // know how to emulate QD3D's bug here.
3715 //
3716 // Reference for this code:
3717 //
3718 // http://www.gamasutra.com/features/19980703/quaternions_01.htm
3719 //-----------------------------------------------------------------------------
3720 TQ3Quaternion *
E3Quaternion_SetMatrix(TQ3Quaternion * quaternion,const TQ3Matrix4x4 * matrix4x4)3721 E3Quaternion_SetMatrix(TQ3Quaternion *quaternion, const TQ3Matrix4x4 *matrix4x4)
3722 {
3723 float tr, s, q[4];
3724 TQ3Uns32 i, j, k;
3725
3726 TQ3Uns32 nxt[3] = {1, 2, 0};
3727
3728 #define M(x,y) matrix4x4->value[x][y]
3729
3730 tr = M(0,0) + M(1,1) + M(2,2);
3731
3732 // check the diagonal
3733 if (tr > 0.0f) {
3734 s = E3Math_SquareRoot(tr + 1.0f);
3735 quaternion->w = s / 2.0f;
3736 s = 0.5f / s;
3737 quaternion->x = (M(1,2) - M(2,1)) * s;
3738 quaternion->y = (M(2,0) - M(0,2)) * s;
3739 quaternion->z = (M(0,1) - M(1,0)) * s;
3740 }
3741 else
3742 {
3743 // diagonal is negative
3744 i = 0;
3745 if (M(1,1) > M(0,0)) i = 1;
3746 if (M(2,2) > M(i,i)) i = 2;
3747 j = nxt[i];
3748 k = nxt[j];
3749
3750 s = E3Math_SquareRoot((M(i,i) - (M(j,j) + M(k,k))) + 1.0f);
3751
3752 q[i] = s * 0.5f;
3753
3754 if (s != 0.0f) s = 0.5f / s;
3755
3756 q[3] = (M(j,k) - M(k,j)) * s;
3757 q[j] = (M(i,j) + M(j,i)) * s;
3758 q[k] = (M(i,k) + M(k,i)) * s;
3759
3760 quaternion->x = q[0];
3761 quaternion->y = q[1];
3762 quaternion->z = q[2];
3763 quaternion->w = q[3];
3764 }
3765
3766 #undef M
3767
3768 return(quaternion);
3769 }
3770
3771
3772
3773
3774
3775 //=============================================================================
3776 // E3Quaternion_Copy : Copy quaternion.
3777 //-----------------------------------------------------------------------------
3778 // Note : 'result' may be the same as 'quaternion'.
3779 //-----------------------------------------------------------------------------
3780 TQ3Quaternion *
E3Quaternion_Copy(const TQ3Quaternion * quaternion,TQ3Quaternion * result)3781 E3Quaternion_Copy(const TQ3Quaternion *quaternion, TQ3Quaternion *result)
3782 {
3783 Q3FastQuaternion_Copy(quaternion, result);
3784 return(result);
3785 }
3786
3787
3788
3789
3790
3791 //=============================================================================
3792 // E3Quaternion_IsIdentity : Return if quaternion is (1,0,0,0).
3793 //-----------------------------------------------------------------------------
3794 // Note : The QuickDraw 3D 1.6 version appears to tolerate x, y or z
3795 // within FLT_EPSILON of 0.
3796 //
3797 // Note : For correct results, |quaternion] == 1.
3798 //-----------------------------------------------------------------------------
3799 TQ3Boolean
E3Quaternion_IsIdentity(const TQ3Quaternion * quaternion)3800 E3Quaternion_IsIdentity(const TQ3Quaternion *quaternion)
3801 {
3802 return((TQ3Boolean)
3803 (
3804 quaternion->x <= FLT_EPSILON && quaternion->x >= -FLT_EPSILON &&
3805 quaternion->y <= FLT_EPSILON && quaternion->y >= -FLT_EPSILON &&
3806 quaternion->z <= FLT_EPSILON && quaternion->z >= -FLT_EPSILON
3807 ));
3808 }
3809
3810
3811
3812
3813
3814 //=============================================================================
3815 // E3Quaternion_Dot : Return dot product of q1 and q2.
3816 //-----------------------------------------------------------------------------
3817 float
E3Quaternion_Dot(const TQ3Quaternion * q1,const TQ3Quaternion * q2)3818 E3Quaternion_Dot(const TQ3Quaternion *q1, const TQ3Quaternion *q2)
3819 {
3820 return(Q3FastQuaternion_Dot(q1, q2));
3821 }
3822
3823
3824
3825
3826
3827 //=============================================================================
3828 // E3Quaternion_Normalize : Scale quaternion to length 1.
3829 //-----------------------------------------------------------------------------
3830 // Note : 'result' may be the same as 'quaternion'.
3831 //-----------------------------------------------------------------------------
3832 TQ3Quaternion *
E3Quaternion_Normalize(const TQ3Quaternion * quaternion,TQ3Quaternion * result)3833 E3Quaternion_Normalize(const TQ3Quaternion *quaternion, TQ3Quaternion *result)
3834 {
3835 Q3FastQuaternion_Normalize(quaternion, result);
3836 return(result);
3837 }
3838
3839
3840
3841
3842
3843 //=============================================================================
3844 // E3Quaternion_Invert : Invert quaternion.
3845 //-----------------------------------------------------------------------------
3846 // Note : 'result' may be the same as 'quaternion'.
3847 //
3848 // For correct results, |quaternion] == 1.
3849 //-----------------------------------------------------------------------------
3850 TQ3Quaternion *
E3Quaternion_Invert(const TQ3Quaternion * quaternion,TQ3Quaternion * result)3851 E3Quaternion_Invert(const TQ3Quaternion *quaternion, TQ3Quaternion *result)
3852 {
3853 Q3FastQuaternion_Invert(quaternion, result);
3854 return(result);
3855 }
3856
3857
3858
3859
3860
3861 //=============================================================================
3862 // E3Quaternion_Multiply : Multiply 'q2' by 'q1'.
3863 //-----------------------------------------------------------------------------
3864 // Note : 'result' may be the same as 'q1' and/or 'q2'.
3865 //
3866 // There is another algorithm which reduces the multiplies
3867 // but increases the adds. Probably not a big difference on
3868 // the PowerPC.
3869 //
3870 // This function actually multiplies the two quaternions in
3871 // the reverse order. Since Quesa uses row vectors rather than
3872 // the more conventional column vectors, we multiply in the
3873 // opposite order: row*matrix rather matrix*column. In order
3874 // to make quaternion and matrix multiplication correspond, it
3875 // is necessary to include this "hack" which allows us to
3876 // to think of composing transforms/matrices/quaternions
3877 // from left-to-right while implementing the conventional
3878 // quaternion composition from right-to-left.
3879 //-----------------------------------------------------------------------------
3880 TQ3Quaternion *
E3Quaternion_Multiply(const TQ3Quaternion * q1,const TQ3Quaternion * q2,TQ3Quaternion * result)3881 E3Quaternion_Multiply(const TQ3Quaternion *q1, const TQ3Quaternion *q2, TQ3Quaternion *result)
3882 {
3883 // If result is alias of input, output to temporary
3884 TQ3Quaternion temp;
3885 TQ3Quaternion* output = (result == q1 || result == q2 ? &temp : result);
3886
3887 // Forward multiplication (q1 * q2)
3888 /*
3889 result->w = q1->w*q2->w - q1->x*q2->x - q1->y*q2->y - q1->z*q2->z;
3890 result->x = q1->w*q2->x + q1->x*q2->w + q1->y*q2->z - q1->z*q2->y;
3891 result->y = q1->w*q2->y + q1->y*q2->w + q1->z*q2->x - q1->x*q2->z;
3892 result->z = q1->w*q2->z + q1->z*q2->w + q1->x*q2->y - q1->y*q2->x;
3893 */
3894
3895 // Reverse multiplication (q2 * q1)
3896 output->w = q1->w*q2->w - q1->x*q2->x - q1->y*q2->y - q1->z*q2->z;
3897 output->x = q1->w*q2->x + q1->x*q2->w - q1->y*q2->z + q1->z*q2->y;
3898 output->y = q1->w*q2->y + q1->y*q2->w - q1->z*q2->x + q1->x*q2->z;
3899 output->z = q1->w*q2->z + q1->z*q2->w - q1->x*q2->y + q1->y*q2->x;
3900
3901 if (output == &temp)
3902 *result = temp;
3903
3904 return(result);
3905 }
3906
3907
3908
3909
3910
3911 //=============================================================================
3912 // E3Quaternion_MatchReflection : sets result to either q1 or -q1,
3913 // whichever produces a positive dot product with q2.
3914 //-----------------------------------------------------------------------------
3915 // Note : Untested.
3916 //-----------------------------------------------------------------------------
3917 TQ3Quaternion *
E3Quaternion_MatchReflection(const TQ3Quaternion * q1,const TQ3Quaternion * q2,TQ3Quaternion * result)3918 E3Quaternion_MatchReflection(const TQ3Quaternion *q1, const TQ3Quaternion *q2,
3919 TQ3Quaternion *result)
3920 {
3921 float dot = q1->w*q2->w + q1->x*q2->x + q1->y*q2->y + q1->z*q2->z;
3922 if (dot > 0) *result = *q1;
3923 else {
3924 result->w = -q1->w;
3925 result->x = -q1->x;
3926 result->y = -q1->y;
3927 result->z = -q1->z;
3928 }
3929
3930 return(result);
3931 }
3932
3933
3934
3935
3936
3937 //=============================================================================
3938 // E3Quaternion_InterpolateFast : Do a straight linear interpolation.
3939 //-----------------------------------------------------------------------------
3940 // Note : This does a true linear, not spherical, interpolation between
3941 // q1 and q2. Fast, but not very proper for most uses.
3942 //
3943 // Untested.
3944 //-----------------------------------------------------------------------------
3945 TQ3Quaternion *
E3Quaternion_InterpolateFast(const TQ3Quaternion * q1,const TQ3Quaternion * q2,float t,TQ3Quaternion * result)3946 E3Quaternion_InterpolateFast(const TQ3Quaternion *q1, const TQ3Quaternion *q2, float t, TQ3Quaternion *result)
3947 {
3948 float t1 = 1.0f - t;
3949 result->w = q1->w*t1 + q2->w*t;
3950 result->x = q1->x*t1 + q2->x*t;
3951 result->y = q1->y*t1 + q2->y*t;
3952 result->z = q1->z*t1 + q2->z*t;
3953 Q3Quaternion_Normalize(result, result);
3954 return(result);
3955 }
3956
3957
3958
3959
3960
3961 //=============================================================================
3962 // E3Quaternion_InterpolateLinear : Spherical linear interpolation.
3963 //-----------------------------------------------------------------------------
3964 // Note : Untested.
3965 //-----------------------------------------------------------------------------
3966 // Note : Despite the name, this function does a SLERP from q1 to q2.
3967 // It falls back on a straight linear interpolation only when the
3968 // cosine of the angle between them is less than 0.01.
3969 //
3970 // The cut-off point was chosen arbitrarily, and may not match
3971 // that of QD3D.
3972 //
3973 // This code adapted from:
3974 // http://www.gamasutra.com/features/19980703/quaternions_01.htm
3975 //
3976 // Untested.
3977 //-----------------------------------------------------------------------------
3978 TQ3Quaternion *
E3Quaternion_InterpolateLinear(const TQ3Quaternion * q1,const TQ3Quaternion * q2,float t,TQ3Quaternion * result)3979 E3Quaternion_InterpolateLinear(const TQ3Quaternion *q1, const TQ3Quaternion *q2, float t, TQ3Quaternion *result)
3980 {
3981 float to1[4];
3982 float omega, cosom, sinom, scale0, scale1;
3983
3984 // calc cosine
3985 cosom = q1->x*q2->x + q1->y*q2->y + q1->z*q2->z
3986 + q1->w*q2->w;
3987
3988 // adjust signs (if necessary)
3989 if ( cosom < 0.0f ){
3990 cosom = -cosom;
3991 to1[0] = - q2->x;
3992 to1[1] = - q2->y;
3993 to1[2] = - q2->z;
3994 to1[3] = - q2->w;
3995 } else {
3996 to1[0] = q2->x;
3997 to1[1] = q2->y;
3998 to1[2] = q2->z;
3999 to1[3] = q2->w;
4000 }
4001
4002 // calculate coefficients
4003
4004 if ( (1.0f - cosom) > 0.01f ) {
4005 // standard case (slerp)
4006 omega = (float) acos(cosom);
4007 sinom = (float) sin(omega);
4008 scale0 = (float) sin((1.0f - t) * omega) / sinom;
4009 scale1 = (float) sin(t * omega) / sinom;
4010 } else {
4011 // "q1" and "q2" quaternions are very close
4012 // ... so we can do a linear interpolation
4013 scale0 = 1.0f - t;
4014 scale1 = t;
4015 }
4016 // calculate final values
4017 result->x = scale0*q1->x + scale1*to1[0];
4018 result->y = scale0*q1->y + scale1*to1[1];
4019 result->z = scale0*q1->z + scale1*to1[2];
4020 result->w = scale0*q1->w + scale1*to1[3];
4021 return(result);
4022 }
4023
4024
4025
4026
4027
4028 //=============================================================================
4029 // E3Quaternion_GetAxisAndAngle : Get the rotation axis and angle
4030 // represented by a quaternion.
4031 //-----------------------------------------------------------------------------
4032 // Note : 'outAxis' or 'outAngle' may be NULL.
4033 //
4034 // For correct results, |quaternion] == 1.
4035 //
4036 // References:
4037 // http://www.cs.ualberta.ca/~andreas/math/matrfaq_latest.html
4038 // book: "3D Game Engine Design"
4039 //-----------------------------------------------------------------------------
4040 TQ3Vector3D *
E3Quaternion_GetAxisAndAngle(const TQ3Quaternion * q,TQ3Vector3D * outAxis,float * outAngle)4041 E3Quaternion_GetAxisAndAngle(const TQ3Quaternion *q, TQ3Vector3D *outAxis, float *outAngle)
4042 {
4043 if (q->w > 1.0f - kQ3RealZero || q->w < -1.0f + kQ3RealZero)
4044 {
4045 // |w| = 1 means this is a null rotation.
4046 // Return 0 for the angle, and pick an arbitrary axis.
4047 if (outAngle) *outAngle = 0.0f;
4048 if (outAxis)
4049 {
4050 outAxis->x = 0.0f;
4051 outAxis->y = 1.0f;
4052 outAxis->z = 0.0f;
4053 }
4054 }
4055 else
4056 {
4057 // |w| != 1, so we have a non-null rotation.
4058 // w is the cosine of the half-angle.
4059 float coshalf = q->w;
4060 if (outAngle)
4061 *outAngle = 2.0f * (float) acos(coshalf);
4062 if (outAxis)
4063 {
4064 float sinhalf = E3Math_SquareRoot(1.0f - coshalf * coshalf);
4065 outAxis->x = q->x / sinhalf;
4066 outAxis->y = q->y / sinhalf;
4067 outAxis->z = q->z / sinhalf;
4068 }
4069 }
4070
4071 return (outAxis);
4072 }
4073
4074
4075
4076
4077
4078 //=============================================================================
4079 // E3Vector3D_TransformQuaternion : Transform 3D vector by quaternion.
4080 //-----------------------------------------------------------------------------
4081 // Note : 'result' may be the same as 'vector3D'.
4082 //
4083 // For correct results, |quaternion] == 1.
4084 //
4085 // http://www.gamasutra.com/features/19980703/quaternions_01.htm
4086 //
4087 // Here we do the multiplication by hand (in the conventional
4088 // forward order), gaining efficiency by making use of the fact
4089 // that the w component of a vector is 0.
4090 //
4091 // If we were to call E3Quaternion_Multiply, we would have to
4092 // compensate for the fact that that function mutiplies in the
4093 // reverse order.
4094 //-----------------------------------------------------------------------------
4095 TQ3Vector3D *
E3Vector3D_TransformQuaternion(const TQ3Vector3D * vector3D,const TQ3Quaternion * quaternion,TQ3Vector3D * result)4096 E3Vector3D_TransformQuaternion(const TQ3Vector3D *vector3D, const TQ3Quaternion *quaternion,
4097 TQ3Vector3D *result)
4098 {
4099 TQ3Quaternion temp;
4100
4101 // Multiply quaternion by vector3D (noting that vector3D->w is 0)
4102 temp.w = quaternion->x*vector3D->x + quaternion->y*vector3D->y + quaternion->z*vector3D->z;
4103 temp.x = quaternion->w*vector3D->x + quaternion->y*vector3D->z - quaternion->z*vector3D->y;
4104 temp.y = quaternion->w*vector3D->y + quaternion->z*vector3D->x - quaternion->x*vector3D->z;
4105 temp.z = quaternion->w*vector3D->z + quaternion->x*vector3D->y - quaternion->y*vector3D->x;
4106
4107 // Multiply (quaternion*vector3D) by inverse(quaternion) (noting that result->w is 0)
4108 result->x = temp.w*quaternion->x + temp.x*quaternion->w - (temp.y*quaternion->z - temp.z*quaternion->y);
4109 result->y = temp.w*quaternion->y + temp.y*quaternion->w + (temp.x*quaternion->z - temp.z*quaternion->x);
4110 result->z = temp.w*quaternion->z + temp.z*quaternion->w - (temp.x*quaternion->y - temp.y*quaternion->x);
4111
4112 return(result);
4113 }
4114
4115
4116
4117
4118
4119 //=============================================================================
4120 // E3Point3D_TransformQuaternion : Transform 3D point by quaternion.
4121 //-----------------------------------------------------------------------------
4122 // Note : Equivalent to E3Vector3D_TransformQuaternion.
4123 //-----------------------------------------------------------------------------
4124 TQ3Point3D *
E3Point3D_TransformQuaternion(const TQ3Point3D * point3D,const TQ3Quaternion * quaternion,TQ3Point3D * result)4125 E3Point3D_TransformQuaternion(const TQ3Point3D *point3D, const TQ3Quaternion *quaternion,
4126 TQ3Point3D *result)
4127 {
4128 return((TQ3Point3D*)
4129 E3Vector3D_TransformQuaternion((const TQ3Vector3D*) point3D, quaternion,
4130 (TQ3Vector3D*) result));
4131 }
4132
4133
4134
4135
4136
4137 //=============================================================================
4138 // E3BoundingBox_Reset : Reset (set to empty) bounding box.
4139 //-----------------------------------------------------------------------------
4140 #pragma mark -
4141 TQ3BoundingBox *
E3BoundingBox_Reset(TQ3BoundingBox * bBox)4142 E3BoundingBox_Reset(TQ3BoundingBox *bBox)
4143 {
4144 Q3FastBoundingBox_Reset(bBox);
4145 return(bBox);
4146 }
4147
4148
4149
4150
4151
4152 //=============================================================================
4153 // E3BoundingBox_Set : Set bounding box.
4154 //-----------------------------------------------------------------------------
4155 TQ3BoundingBox *
E3BoundingBox_Set(TQ3BoundingBox * bBox,const TQ3Point3D * min,const TQ3Point3D * max,TQ3Boolean isEmpty)4156 E3BoundingBox_Set(TQ3BoundingBox *bBox, const TQ3Point3D *min, const TQ3Point3D *max, TQ3Boolean isEmpty)
4157 {
4158 Q3FastBoundingBox_Set(bBox, min, max, isEmpty);
4159 return(bBox);
4160 }
4161
4162
4163
4164
4165
4166 //=============================================================================
4167 // E3BoundingBox_SetFromPoints3D : Set bounding box to just enclose set
4168 // of 3D points.
4169 //-----------------------------------------------------------------------------
4170 TQ3BoundingBox *
E3BoundingBox_SetFromPoints3D(TQ3BoundingBox * bBox,const TQ3Point3D * points3D,TQ3Uns32 numPoints,TQ3Uns32 structSize)4171 E3BoundingBox_SetFromPoints3D(TQ3BoundingBox *bBox,
4172 const TQ3Point3D *points3D, TQ3Uns32 numPoints, TQ3Uns32 structSize)
4173 {
4174 if (numPoints == 0)
4175 Q3BoundingBox_Reset(bBox);
4176 else
4177 {
4178 const char* in = (const char*) points3D;
4179 TQ3Uns32 i;
4180
4181 Q3BoundingBox_Set(bBox, points3D, points3D, kQ3False);
4182 in += structSize;
4183
4184 for (i = 1; i < numPoints; ++i, in += structSize)
4185 e3bounding_box_accumulate_point3D(bBox, (const TQ3Point3D*) in);
4186 }
4187
4188 return(bBox);
4189 }
4190
4191
4192
4193
4194
4195 //=============================================================================
4196 // E3BoundingBox_SetFromRationalPoints4D : Set bounding box to just enclose
4197 // set of 4D rational points.
4198 //-----------------------------------------------------------------------------
4199 TQ3BoundingBox *
E3BoundingBox_SetFromRationalPoints4D(TQ3BoundingBox * bBox,const TQ3RationalPoint4D * rationalPoints4D,TQ3Uns32 numPoints,TQ3Uns32 structSize)4200 E3BoundingBox_SetFromRationalPoints4D(TQ3BoundingBox *bBox,
4201 const TQ3RationalPoint4D *rationalPoints4D, TQ3Uns32 numPoints, TQ3Uns32 structSize)
4202 {
4203 if (numPoints == 0)
4204 Q3BoundingBox_Reset(bBox);
4205 else
4206 {
4207 TQ3Point3D point3D;
4208 const char* in = (const char*) rationalPoints4D;
4209 TQ3Uns32 i;
4210
4211 Q3RationalPoint4D_To3D(rationalPoints4D, &point3D);
4212 Q3BoundingBox_Set(bBox, &point3D, &point3D, kQ3False);
4213 in += structSize;
4214
4215 for (i = 1; i < numPoints; ++i, in += structSize)
4216 {
4217 Q3RationalPoint4D_To3D((const TQ3RationalPoint4D*) in, &point3D);
4218 e3bounding_box_accumulate_point3D(bBox, &point3D);
4219 }
4220 }
4221
4222 return(bBox);
4223 }
4224
4225
4226
4227
4228
4229 //=============================================================================
4230 // E3BoundingBox_Copy : Copy bounding box.
4231 //-----------------------------------------------------------------------------
4232 // Note : 'result' may be the same as 'bBox'.
4233 //-----------------------------------------------------------------------------
4234 TQ3BoundingBox *
E3BoundingBox_Copy(const TQ3BoundingBox * bBox,TQ3BoundingBox * result)4235 E3BoundingBox_Copy(const TQ3BoundingBox *bBox, TQ3BoundingBox *result)
4236 {
4237 Q3FastBoundingBox_Copy(bBox, result);
4238 return(result);
4239 }
4240
4241
4242
4243
4244
4245 //=============================================================================
4246 // E3BoundingBox_Union : Return minimum bounding box that encloses both
4247 // 'b1' and 'b2'.
4248 //-----------------------------------------------------------------------------
4249 // Note : 'result' may be the same as 'b1' or 'b2'.
4250 //-----------------------------------------------------------------------------
4251 TQ3BoundingBox *
E3BoundingBox_Union(const TQ3BoundingBox * b1,const TQ3BoundingBox * b2,TQ3BoundingBox * result)4252 E3BoundingBox_Union(const TQ3BoundingBox *b1, const TQ3BoundingBox *b2,
4253 TQ3BoundingBox *result)
4254 {
4255 if (b1->isEmpty)
4256 {
4257 if (b2->isEmpty)
4258 Q3BoundingBox_Reset(result);
4259 else
4260 Q3BoundingBox_Copy(b2, result);
4261 }
4262 else
4263 {
4264 if (b2->isEmpty)
4265 Q3BoundingBox_Copy(b1, result);
4266 else
4267 {
4268 result->min.x = E3Num_Min(b1->min.x, b2->min.x);
4269 result->min.y = E3Num_Min(b1->min.y, b2->min.y);
4270 result->min.z = E3Num_Min(b1->min.z, b2->min.z);
4271 result->max.x = E3Num_Max(b1->max.x, b2->max.x);
4272 result->max.y = E3Num_Max(b1->max.y, b2->max.y);
4273 result->max.z = E3Num_Max(b1->max.z, b2->max.z);
4274 result->isEmpty = kQ3False;
4275 }
4276 }
4277
4278 return(result);
4279 }
4280
4281
4282
4283
4284
4285 //=============================================================================
4286 // E3BoundingBox_UnionPoint3D : Return minimum bounding box that encloses
4287 // both 'bBox' and 'point3D'.
4288 //-----------------------------------------------------------------------------
4289 // Note : 'result' may be the same as 'bBox'.
4290 //-----------------------------------------------------------------------------
4291 TQ3BoundingBox *
E3BoundingBox_UnionPoint3D(const TQ3BoundingBox * bBox,const TQ3Point3D * point3D,TQ3BoundingBox * result)4292 E3BoundingBox_UnionPoint3D(const TQ3BoundingBox *bBox, const TQ3Point3D *point3D,
4293 TQ3BoundingBox *result)
4294 {
4295 if (bBox->isEmpty)
4296 Q3BoundingBox_Set(result, point3D, point3D, kQ3False);
4297 else
4298 {
4299 if (result != bBox)
4300 Q3BoundingBox_Copy(bBox, result);
4301
4302 e3bounding_box_accumulate_point3D(result, point3D);
4303 }
4304
4305 return(result);
4306 }
4307
4308
4309
4310
4311
4312 //=============================================================================
4313 // E3BoundingBox_UnionRationalPoint4D : Return minimum bounding box that
4314 // encloses both 'bBox' and
4315 // 'rationalPoint4D'.
4316 //-----------------------------------------------------------------------------
4317 TQ3BoundingBox *
E3BoundingBox_UnionRationalPoint4D(const TQ3BoundingBox * bBox,const TQ3RationalPoint4D * rationalPoint4D,TQ3BoundingBox * result)4318 E3BoundingBox_UnionRationalPoint4D(const TQ3BoundingBox *bBox,
4319 const TQ3RationalPoint4D *rationalPoint4D, TQ3BoundingBox *result)
4320 {
4321 TQ3Point3D point3D;
4322
4323 Q3RationalPoint4D_To3D(rationalPoint4D, &point3D);
4324
4325 return(Q3BoundingBox_UnionPoint3D(bBox, &point3D, result));
4326 }
4327
4328
4329
4330
4331
4332 //=============================================================================
4333 // E3BoundingSphere_Reset : Reset (set to empty) bounding sphere.
4334 //-----------------------------------------------------------------------------
4335 #pragma mark -
4336 TQ3BoundingSphere *
E3BoundingSphere_Reset(TQ3BoundingSphere * bSphere)4337 E3BoundingSphere_Reset(TQ3BoundingSphere *bSphere)
4338 {
4339 Q3FastBoundingSphere_Reset(bSphere);
4340 return(bSphere);
4341 }
4342
4343
4344
4345
4346
4347 //=============================================================================
4348 // E3BoundingSphere_Set : Set bounding sphere.
4349 //-----------------------------------------------------------------------------
4350 TQ3BoundingSphere *
E3BoundingSphere_Set(TQ3BoundingSphere * bSphere,const TQ3Point3D * origin,float radius,TQ3Boolean isEmpty)4351 E3BoundingSphere_Set(TQ3BoundingSphere *bSphere, const TQ3Point3D *origin, float radius, TQ3Boolean isEmpty)
4352 {
4353 Q3FastBoundingSphere_Set(bSphere, origin, radius, isEmpty);
4354 return(bSphere);
4355 }
4356
4357
4358
4359
4360
4361 //=============================================================================
4362 // E3BoundingSphere_SetFromPoints3D : Set bounding sphere to just enclose
4363 // set of 3D points.
4364 //-----------------------------------------------------------------------------
4365 // Note : This implementation is inefficient, and could be improved.
4366 //
4367 // Untested.
4368 //-----------------------------------------------------------------------------
4369 TQ3BoundingSphere *
E3BoundingSphere_SetFromPoints3D(TQ3BoundingSphere * bSphere,const TQ3Point3D * points3D,TQ3Uns32 numPoints,TQ3Uns32 structSize)4370 E3BoundingSphere_SetFromPoints3D(TQ3BoundingSphere *bSphere, const TQ3Point3D *points3D, TQ3Uns32 numPoints, TQ3Uns32 structSize)
4371 {
4372 switch (numPoints)
4373 {
4374 case 0:
4375 Q3BoundingSphere_Reset(bSphere);
4376 break;
4377
4378 case 1:
4379 Q3BoundingSphere_Set(bSphere, points3D, 0.0f, kQ3False);
4380 break;
4381
4382 default:
4383 {
4384 TQ3BoundingBox bBox;
4385
4386 TQ3Point3D origin;
4387 float radiusSquared;
4388 TQ3Uns32 i;
4389 const TQ3Point3D *currPoint3D;
4390
4391 // Determine the bounding box of the specified points
4392 Q3BoundingBox_SetFromPoints3D(&bBox, points3D, numPoints, structSize);
4393
4394 // Set the (initial) origin of the bounding sphere to the center of the bounding box
4395 Q3Point3D_RRatio(&bBox.min, &bBox.max, 0.5f, 0.5f, &origin);
4396
4397 // Set the (initial) radius of the bounding sphere to the maximum distance from the origin
4398 radiusSquared = 0.0f;
4399 for (i = 0, currPoint3D = points3D; i < numPoints; ++i, ((const char *&) currPoint3D) += structSize)
4400 {
4401 float currRadiusSquared = Q3Point3D_DistanceSquared(&origin, currPoint3D);
4402
4403 if (currRadiusSquared > radiusSquared)
4404 radiusSquared = currRadiusSquared;
4405 }
4406
4407 Q3BoundingSphere_Set(bSphere, &origin, Q3Math_SquareRoot(radiusSquared), kQ3False);
4408 }
4409 break;
4410 }
4411
4412 return(bSphere);
4413 }
4414
4415
4416
4417
4418
4419 //=============================================================================
4420 // E3BoundingSphere_SetFromRationalPoints4D : Set bounding sphere to just
4421 // enclose set of 4D rational
4422 // points.
4423 //-----------------------------------------------------------------------------
4424 // Note : This implementation is inefficient, and could be improved.
4425 // Also note: QD3D does not produce the same answer as this
4426 // function for at least some inputs.
4427 //
4428 // I think QD3D is wrong, and don't know exactly what it thinks
4429 // it's doing.
4430 //
4431 // Untested.
4432 //-----------------------------------------------------------------------------
4433 TQ3BoundingSphere *
E3BoundingSphere_SetFromRationalPoints4D(TQ3BoundingSphere * bSphere,const TQ3RationalPoint4D * rationalPoints4D,TQ3Uns32 numPoints,TQ3Uns32 structSize)4434 E3BoundingSphere_SetFromRationalPoints4D(TQ3BoundingSphere *bSphere, const TQ3RationalPoint4D *rationalPoints4D, TQ3Uns32 numPoints, TQ3Uns32 structSize)
4435 {
4436 if (numPoints == 0)
4437 Q3BoundingSphere_Reset(bSphere);
4438 else
4439 {
4440 TQ3Point3D point3D;
4441 const char* in = (const char*) rationalPoints4D;
4442 TQ3Uns32 i;
4443
4444 Q3RationalPoint4D_To3D(rationalPoints4D, &point3D);
4445 Q3BoundingSphere_Set(bSphere, &point3D, 0.0f, kQ3False);
4446 in += structSize;
4447
4448 for (i = 1; i < numPoints; ++i, in += structSize)
4449 {
4450 Q3RationalPoint4D_To3D((const TQ3RationalPoint4D*) in, &point3D);
4451 Q3BoundingSphere_UnionPoint3D(bSphere, &point3D, bSphere);
4452 }
4453 }
4454
4455 return(bSphere);
4456 }
4457
4458
4459
4460
4461
4462 //=============================================================================
4463 // E3BoundingSphere_Copy : Copy bounding sphere.
4464 //-----------------------------------------------------------------------------
4465 // Note : 'result' may be the same as 'bSphere'.
4466 //-----------------------------------------------------------------------------
4467 TQ3BoundingSphere *
E3BoundingSphere_Copy(const TQ3BoundingSphere * bSphere,TQ3BoundingSphere * result)4468 E3BoundingSphere_Copy(const TQ3BoundingSphere *bSphere, TQ3BoundingSphere *result)
4469 {
4470 Q3FastBoundingSphere_Copy(bSphere, result);
4471 return(result);
4472 }
4473
4474
4475
4476
4477
4478 //=============================================================================
4479 // E3BoundingSphere_Union : Return minimum bounding sphere that encloses
4480 // both 's1' and 's2'.
4481 //-----------------------------------------------------------------------------
4482 // Note : 'result' may be the same as 's1' or 's2'.
4483 //
4484 // Untested.
4485 //-----------------------------------------------------------------------------
4486 TQ3BoundingSphere *
E3BoundingSphere_Union(const TQ3BoundingSphere * s1,const TQ3BoundingSphere * s2,TQ3BoundingSphere * result)4487 E3BoundingSphere_Union(const TQ3BoundingSphere *s1, const TQ3BoundingSphere *s2,
4488 TQ3BoundingSphere *result)
4489 {
4490
4491
4492 if (s1->isEmpty)
4493 {
4494 if (s2->isEmpty)
4495 Q3FastBoundingSphere_Reset(result);
4496 else
4497 Q3FastBoundingSphere_Copy(s2, result);
4498 }
4499 else
4500 {
4501 if (s2->isEmpty)
4502 {
4503 Q3FastBoundingSphere_Copy(s1, result);
4504 }
4505 else
4506 {
4507 float dist = Q3FastPoint3D_Distance(&s1->origin, &s2->origin);
4508
4509 if (dist > kQ3RealZero)
4510 {
4511 float finalRadius = (dist + s1->radius + s2->radius)/2.0f;
4512 finalRadius = E3Num_Max(s1->radius,finalRadius);
4513 finalRadius = E3Num_Max(s2->radius,finalRadius);
4514
4515 Q3FastPoint3D_RRatio(&s1->origin, &s2->origin,
4516 ((finalRadius - s2->radius)/dist),
4517 ((dist - (finalRadius - s2->radius))/dist),
4518 &result->origin);
4519
4520 result->radius = finalRadius;
4521 }
4522 else
4523 {
4524 // if the points are the same, then just take whichever radius is greater.
4525 result->origin = s1->origin;
4526 result->radius = E3Num_Max(s1->radius, s2->radius);
4527 }
4528 result->isEmpty = kQ3False;
4529 }
4530 }
4531
4532 return(result);
4533 }
4534
4535
4536
4537
4538
4539 //=============================================================================
4540 // E3BoundingSphere_UnionPoint3D : Return minimum bounding sphere that
4541 // encloses both 'bSphere' and 'point3D'.
4542 //-----------------------------------------------------------------------------
4543 // Note : Untested.
4544 //-----------------------------------------------------------------------------
4545 TQ3BoundingSphere *
E3BoundingSphere_UnionPoint3D(const TQ3BoundingSphere * bSphere,const TQ3Point3D * point3D,TQ3BoundingSphere * result)4546 E3BoundingSphere_UnionPoint3D(const TQ3BoundingSphere *bSphere, const TQ3Point3D *point3D,
4547 TQ3BoundingSphere *result)
4548 {
4549 if (bSphere->isEmpty)
4550 {
4551 result->origin = *point3D;
4552 result->radius = 0.0f;
4553 }
4554 else
4555 {
4556 // Approach here is similar to that for Union,
4557 // if we imagine the given point as a radius=0 sphere.
4558 float x1=bSphere->origin.x, y1=bSphere->origin.y, z1=bSphere->origin.z;
4559 float x2=point3D->x, y2=point3D->y, z2=point3D->z;
4560 // find the deltas between their centers, and the distance.
4561 float dx = x2-x1, dy = y2-y1, dz = z2-z1, dist = Q3Math_SquareRoot(dx*dx + dy*dy + dz*dz);
4562 if (dist > bSphere->radius)
4563 {
4564 // find the far points.
4565 float factor = bSphere->radius / dist;
4566 float fx1 = x1 - dx*factor;
4567 float fy1 = y1 - dy*factor;
4568 float fz1 = z1 - dz*factor;
4569 // finish the job.
4570 result->origin.x = (fx1+x2)/2;
4571 result->origin.y = (fy1+y2)/2;
4572 result->origin.z = (fz1+z2)/2;
4573 dx = fx1-x2;
4574 dy = fy1-y2;
4575 dz = fz1-z2;
4576 result->radius = Q3Math_SquareRoot(dx*dx + dy*dy + dz*dz) / 2.0f;
4577 }
4578 else
4579 {
4580 // if the point is within the sphere, then no change is necessary.
4581 Q3BoundingSphere_Copy(bSphere, result);
4582 return(result);
4583 }
4584 }
4585
4586 result->isEmpty = kQ3False;
4587
4588 return(result);
4589 }
4590
4591
4592
4593
4594
4595 //=============================================================================
4596 // E3BoundingSphere_UnionRationalPoint4D : Return minimum bounding sphere
4597 // that encloses both 'bSphere'
4598 // and 'rationalPoint4D'.
4599 //-----------------------------------------------------------------------------
4600 // Note : Untested.
4601 //-----------------------------------------------------------------------------
4602 TQ3BoundingSphere *
E3BoundingSphere_UnionRationalPoint4D(const TQ3BoundingSphere * bSphere,const TQ3RationalPoint4D * rationalPoint4D,TQ3BoundingSphere * result)4603 E3BoundingSphere_UnionRationalPoint4D(const TQ3BoundingSphere *bSphere,
4604 const TQ3RationalPoint4D *rationalPoint4D, TQ3BoundingSphere *result)
4605 {
4606 TQ3Point3D point3D;
4607
4608 Q3RationalPoint4D_To3D(rationalPoint4D, &point3D);
4609
4610 return(Q3BoundingSphere_UnionPoint3D(bSphere, &point3D, result));
4611 }
4612
4613
4614
4615
4616
4617 //=============================================================================
4618 // E3Ray3D_IntersectSphere : Perform a ray/sphere intersection test.
4619 //-----------------------------------------------------------------------------
4620 // Note : We assume that the ray direction vector has been normalised.
4621 // The algorithm is from 'Real Time Rendering', section 10.3.2.
4622 //-----------------------------------------------------------------------------
4623 #pragma mark -
4624 TQ3Boolean
E3Ray3D_IntersectSphere(const TQ3Ray3D * theRay,const TQ3Sphere * theSphere,TQ3Point3D * hitPoint)4625 E3Ray3D_IntersectSphere(const TQ3Ray3D *theRay, const TQ3Sphere *theSphere, TQ3Point3D *hitPoint)
4626 { TQ3Vector3D sphereToRay, intersectVector;
4627 float d, q, t, l2, r2, m2;
4628
4629
4630
4631 // Prepare to intersect
4632 //
4633 // First calculate the vector from the sphere to the ray origin, its
4634 // length squared, the projection of this vector onto the ray direction,
4635 // and the squared radius of the sphere.
4636 Q3Point3D_Subtract(&theSphere->origin, &theRay->origin, &sphereToRay);
4637 l2 = Q3Vector3D_LengthSquared(&sphereToRay);
4638 d = Q3Vector3D_Dot(&sphereToRay, &theRay->direction);
4639 r2 = theSphere->radius * theSphere->radius;
4640
4641
4642
4643 // If the sphere is behind the ray origin, they don't intersect
4644 if (d < 0.0f && l2 > r2)
4645 return(kQ3False);
4646
4647
4648
4649 // Calculate the squared distance from the sphere center to the projection.
4650 // If it's greater than the radius then they don't intersect.
4651 m2 = (l2 - (d * d));
4652 if (m2 > r2)
4653 return(kQ3False);
4654
4655
4656
4657 // Calculate the distance along the ray to the intersection point
4658 q = E3Math_SquareRoot(r2 - m2);
4659 if (l2 > r2)
4660 t = d - q;
4661 else
4662 t = d + q;
4663
4664
4665
4666 // Calculate the intersection point
4667 Q3Vector3D_Scale(&theRay->direction, t, &intersectVector);
4668 Q3Point3D_Vector3D_Add(&theRay->origin, &intersectVector, hitPoint);
4669
4670 return(kQ3True);
4671 }
4672
4673
4674
4675
4676
4677 //=============================================================================
4678 // E3Ray3D_IntersectBoundingBox : Perform a ray/box intersection test.
4679 //-----------------------------------------------------------------------------
4680 // Note : We assume that the ray direction vector has been normalised.
4681 // The algorithm is Andrew Woo's "Fast Ray-Box Intersection"
4682 // from "Graphics Gems".
4683 //-----------------------------------------------------------------------------
4684 TQ3Boolean
E3Ray3D_IntersectBoundingBox(const TQ3Ray3D * theRay,const TQ3BoundingBox * theBounds,TQ3Point3D * hitPoint)4685 E3Ray3D_IntersectBoundingBox(const TQ3Ray3D *theRay, const TQ3BoundingBox *theBounds, TQ3Point3D *hitPoint)
4686 { float candidatePlane[3], maxT[3], coord[3];
4687 float minB[3], maxB[3], origin[3], dir[3];
4688 TQ3Uns32 i, whichPlane, right, left, middle;
4689 TQ3Int8 quadrant[3];
4690 TQ3Boolean isInside;
4691
4692
4693
4694 // Initialise ourselves
4695 minB[0] = theBounds->min.x;
4696 minB[1] = theBounds->min.y;
4697 minB[2] = theBounds->min.z;
4698
4699 maxB[0] = theBounds->max.x;
4700 maxB[1] = theBounds->max.y;
4701 maxB[2] = theBounds->max.z;
4702
4703 origin[0] = theRay->origin.x;
4704 origin[1] = theRay->origin.y;
4705 origin[2] = theRay->origin.z;
4706
4707 dir[0] = theRay->direction.x;
4708 dir[1] = theRay->direction.y;
4709 dir[2] = theRay->direction.z;
4710
4711 isInside = kQ3True;
4712 right = 0;
4713 left = 1;
4714 middle = 2;
4715
4716
4717
4718 // Find candidate planes
4719 for (i = 0; i < 3; i++)
4720 {
4721 if (origin[i] < minB[i])
4722 {
4723 quadrant[i] = (TQ3Int8) left;
4724 candidatePlane[i] = minB[i];
4725 isInside = kQ3False;
4726 }
4727 else if (origin[i] > maxB[i])
4728 {
4729 quadrant[i] = (TQ3Int8) right;
4730 candidatePlane[i] = maxB[i];
4731 isInside = kQ3False;
4732 }
4733 else
4734 quadrant[i] = (TQ3Int8) middle;
4735 }
4736
4737
4738
4739 // Check for the ray origin being inside the bounding box
4740 if (isInside)
4741 {
4742 if (hitPoint) *hitPoint = theRay->origin;
4743 return(kQ3True);
4744 }
4745
4746
4747
4748 // Calculate T distances to candidate planes
4749 for (i = 0; i < 3; i++)
4750 {
4751 if (quadrant[i] != (TQ3Int8) middle && dir[i] != 0.0f)
4752 maxT[i] = (candidatePlane[i] - origin[i]) / dir[i];
4753 else
4754 maxT[i] = -1.0f;
4755 }
4756
4757
4758
4759 // Get largest of the maxT's for final choice of intersection
4760 whichPlane = 0;
4761 for (i = 1; i < 3; i++)
4762 {
4763 if (maxT[whichPlane] < maxT[i])
4764 whichPlane = i;
4765 }
4766
4767
4768
4769 // Check final candidate actually inside box
4770 if (maxT[whichPlane] < 0.0f)
4771 return(kQ3False);
4772
4773 for (i = 0; i < 3; i++)
4774 {
4775 if (whichPlane != i)
4776 {
4777 coord[i] = origin[i] + maxT[whichPlane] * dir[i];
4778 if (coord[i] < minB[i] || coord[i] > maxB[i])
4779 return(kQ3False);
4780 }
4781 else
4782 coord[i] = candidatePlane[i];
4783 }
4784
4785 if (hitPoint)
4786 {
4787 hitPoint->x = coord[0];
4788 hitPoint->y = coord[1];
4789 hitPoint->z = coord[2];
4790 }
4791
4792 return(kQ3True);
4793 }
4794
4795
4796
4797
4798
4799 //=============================================================================
4800 // E3Ray3D_IntersectTriangle : Perform a ray/triangle intersection test.
4801 //-----------------------------------------------------------------------------
4802 // Note : Uses the Moller-Trumbore algorithm to test for intersection,
4803 // and if found returns the barycentric coordinates of the
4804 // intersection point. The third component of hitPoint, w, is
4805 // used to store the distance along the ray to the plane in
4806 // which the triangle lies.
4807 //
4808 // Details at:
4809 // <http://www.acm.org/jgt/papers/MollerTrumbore97/>.
4810 //-----------------------------------------------------------------------------
4811 TQ3Boolean
E3Ray3D_IntersectTriangle(const TQ3Ray3D * theRay,const TQ3Point3D * point1,const TQ3Point3D * point2,const TQ3Point3D * point3,TQ3Boolean cullBackfacing,TQ3Param3D * hitPoint)4812 E3Ray3D_IntersectTriangle(const TQ3Ray3D *theRay,
4813 const TQ3Point3D *point1,
4814 const TQ3Point3D *point2,
4815 const TQ3Point3D *point3,
4816 TQ3Boolean cullBackfacing,
4817 TQ3Param3D *hitPoint)
4818 { TQ3Vector3D edge1, edge2, tvec, pvec, qvec;
4819 float det, invDet;
4820
4821
4822
4823 // Calculate the two edges which share vertex 1
4824 Q3Point3D_Subtract(point2, point1, &edge1);
4825 Q3Point3D_Subtract(point3, point1, &edge2);
4826
4827
4828
4829 // Begin calculating the determinant - also used to calculate u. If the
4830 // determinant is near zero, the ray lies in the plane of the triangle.
4831 Q3Vector3D_Cross(&theRay->direction, &edge2, &pvec);
4832 det = Q3Vector3D_Dot(&edge1, &pvec);
4833
4834
4835
4836 // Handle triangles with back-face culling
4837 if (cullBackfacing)
4838 {
4839 // Test for ray coinciding with triangle plane
4840 if (det < kQ3RealZero)
4841 return(kQ3False);
4842
4843
4844 // Calculate the distance between vertex 1 and the ray origin
4845 Q3Point3D_Subtract(&theRay->origin, point1, &tvec);
4846
4847
4848 // Calculate u, and test for a miss
4849 hitPoint->u = Q3Vector3D_Dot(&tvec, &pvec);
4850 if (hitPoint->u < 0.0f || hitPoint->u > det)
4851 return(kQ3False);
4852
4853
4854 // Calculate v, and test for a miss
4855 Q3Vector3D_Cross(&tvec, &edge1, &qvec);
4856 hitPoint->v = Q3Vector3D_Dot(&theRay->direction, &qvec);
4857 if (hitPoint->v < 0.0f || (hitPoint->u + hitPoint->v) > det)
4858 return(kQ3False);
4859
4860
4861 // Calculate w, and scale the parameters
4862 hitPoint->w = Q3Vector3D_Dot(&edge2, &qvec);
4863 invDet = 1.0f / det;
4864
4865 hitPoint->w *= invDet;
4866 hitPoint->u *= invDet;
4867 hitPoint->v *= invDet;
4868 }
4869
4870
4871 // Handle triangles with no culling
4872 else
4873 {
4874 // Test for ray coinciding with triangle plane
4875 if (det > -kQ3RealZero && det < kQ3RealZero)
4876 return(kQ3False);
4877
4878 invDet = 1.0f / det;
4879
4880
4881 // Calculate the distance between vertex 1 and the ray origin
4882 Q3Point3D_Subtract(&theRay->origin, point1, &tvec);
4883
4884
4885 // Calculate u, and test for a miss
4886 hitPoint->u = Q3Vector3D_Dot(&tvec, &pvec) * invDet;
4887 if (hitPoint->u < 0.0f || hitPoint->u > 1.0f)
4888 return(kQ3False);
4889
4890
4891 // Calculate v, and test for a miss
4892 Q3Vector3D_Cross(&tvec, &edge1, &qvec);
4893 hitPoint->v = Q3Vector3D_Dot(&theRay->direction, &qvec) * invDet;
4894 if (hitPoint->v < 0.0f || (hitPoint->u + hitPoint->v) > 1.0f)
4895 return(kQ3False);
4896
4897
4898 // Calculate w
4899 hitPoint->w = Q3Vector3D_Dot(&edge2, &qvec) * invDet;
4900 }
4901
4902
4903
4904 // The ray intersects the triangle
4905 return (hitPoint->w >= 0.0f ? kQ3True : kQ3False);
4906 }
4907
4908
4909
4910
4911
4912 //=============================================================================
4913 // E3Math_SquareRoot : Fast square root.
4914 //-----------------------------------------------------------------------------
4915 float
E3Math_SquareRoot(float x)4916 E3Math_SquareRoot(float x)
4917 {
4918
4919
4920 // For now, just call sqrt
4921 //
4922 // Should be extended with platform-specific optimisations, and
4923 // possibly a table-based system for all platforms with IEEE floats?
4924 return((float) sqrt(x));
4925 }
4926
4927
4928
4929
4930
4931 //=============================================================================
4932 // E3Math_InvSquareRoot : Fast inverse square root.
4933 //-----------------------------------------------------------------------------
4934 float
E3Math_InvSquareRoot(float x)4935 E3Math_InvSquareRoot(float x)
4936 {
4937
4938
4939 // For now, just call sqrt
4940 //
4941 // Should be extended with platform-specific optimisations, and
4942 // possibly a table-based system for all platforms with IEEE floats?
4943 return(1.0f / (float) sqrt(x));
4944 }
4945
4946