Richard Boegli's CnC_Generals_Zero_Hour Fork WIP
This is documentation of Richard Boegil's Zero Hour Fork
 
Loading...
Searching...
No Matches
quat.cpp
Go to the documentation of this file.
1/*
2** Command & Conquer Generals Zero Hour(tm)
3** Copyright 2025 Electronic Arts Inc.
4**
5** This program is free software: you can redistribute it and/or modify
6** it under the terms of the GNU General Public License as published by
7** the Free Software Foundation, either version 3 of the License, or
8** (at your option) any later version.
9**
10** This program is distributed in the hope that it will be useful,
11** but WITHOUT ANY WARRANTY; without even the implied warranty of
12** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13** GNU General Public License for more details.
14**
15** You should have received a copy of the GNU General Public License
16** along with this program. If not, see <http://www.gnu.org/licenses/>.
17*/
18
19/* $Header: /Commando/Code/wwmath/quat.cpp 38 8/28/01 10:26a Greg_h $ */
20/***********************************************************************************************
21 *** Confidential - Westwood Studios ***
22 ***********************************************************************************************
23 * *
24 * Project Name : Voxel Technology *
25 * *
26 * File Name : QUAT.CPP *
27 * *
28 * Programmer : Greg Hjelstrom *
29 * *
30 * Start Date : 02/24/97 *
31 * *
32 * Last Update : February 28, 1997 [GH] *
33 * *
34 *---------------------------------------------------------------------------------------------*
35 * Functions: *
36 * Quaternion::Quaternion -- constructor *
37 * Quaternion::Set -- Set the quaternion *
38 * Quaternion::operator= -- Assignment operator *
39 * Quaternion::Make_Closest -- Use nearest representation to the given quaternion. *
40 * Trackball -- Computes a "trackball" quaternion given 2D mouse coordinates *
41 * Axis_To_Quat -- Creates a quaternion given an axis and angle of rotation *
42 * Slerp -- Spherical Linear interpolation! *
43 * Build_Quaternion -- Creates a quaternion from a Matrix *
44 * Build_Matrix -- Creates a Matrix from a Quaternion *
45 * Normalize -- normalizes a quaternion *
46 * Quaternion::Quaternion -- constructor *
47 * Slerp_Setup -- Get ready to call "Cached_Slerp" *
48 * Cached_Slerp -- Quaternion slerping, optimized with cached values *
49 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
50
51
52#include "quat.h"
53#include "matrix3d.h"
54#include "matrix4.h"
55#include "wwmath.h"
56
57#include <stdio.h>
58#include <stdlib.h>
59#include <math.h>
60#include <assert.h>
61
62#define SLERP_EPSILON 0.001
63
64static int _nxt[3] = { 1 , 2 , 0 };
65
66
67// ------------------------------------------------------------
68// local functions
69// ------------------------------------------------------------
70static float project_to_sphere(float,float,float);
71
72
73/***********************************************************************************************
74 * Quaternion::Quaternion -- constructor *
75 * *
76 * constructs a quaternion from the given axis and angle of rotation (in RADIANS of course) *
77 * *
78 * INPUT: *
79 * axis - axis of the rotation *
80 * angle - rotation angle *
81 * *
82 * OUTPUT: *
83 * *
84 * WARNINGS: *
85 * *
86 * HISTORY: *
87 * 12/10/97 GTH : Created. *
88 *=============================================================================================*/
89Quaternion::Quaternion(const Vector3 & axis,float angle)
90{
91 float s = WWMath::Sin(angle/2);
92 float c = WWMath::Cos(angle/2);
93 X = s * axis.X;
94 Y = s * axis.Y;
95 Z = s * axis.Z;
96 W = c;
97}
98
99
100/***********************************************************************************************
101 * Quaternion::Normalize -- Normalize to a unit quaternion *
102 * *
103 * INPUT: *
104 * *
105 * OUTPUT: *
106 * *
107 * WARNINGS: *
108 * *
109 * HISTORY: *
110 * 02/24/1997 GH : Created. *
111 *=============================================================================================*/
112void Quaternion::Normalize()
113{
114 float len2=X * X + Y * Y + Z * Z + W * W;
115 if (0.0f == len2) {
116 return;
117 } else {
118 float inv_mag = WWMath::Inv_Sqrt(len2);
119
120 X *= inv_mag;
121 Y *= inv_mag;
122 Z *= inv_mag;
123 W *= inv_mag;
124 }
125}
126
127/***********************************************************************************************
128 * Q::Make_Closest -- Use nearest representation to the given quaternion. *
129 * *
130 * INPUT: *
131 * *
132 * OUTPUT: *
133 * *
134 * WARNINGS: *
135 * *
136 * HISTORY: *
137 * 02/28/1997 GH : Created. *
138 *=============================================================================================*/
139Quaternion & Quaternion::Make_Closest(const Quaternion & qto)
140{
141 float cos_t = qto.X * X + qto.Y * Y + qto.Z * Z + qto.W * W;
142
143 // if we are on opposite hemisphere from qto, negate ourselves
144 if (cos_t < 0.0) {
145 X = -X;
146 Y = -Y;
147 Z = -Z;
148 W = -W;
149 }
150
151 return *this;
152}
153
154/***********************************************************************************************
155 * Trackball -- Computes a "trackball" quaternion given 2D mouse coordinates *
156 * *
157 * INPUT: *
158 * x0,y0 - x1,y1 - "normalized" mouse coordinates for the mouse movement *
159 * sphsize - size of the trackball sphere *
160 * *
161 * OUTPUT: *
162 * a quaternion representing the rotation of a trackball *
163 * *
164 * WARNINGS: *
165 * *
166 * HISTORY: *
167 * 02/28/1997 GH : Created. *
168 *=============================================================================================*/
169Quaternion Trackball(float x0, float y0, float x1, float y1, float sphsize)
170{
171 Vector3 a;
172 Vector3 p1;
173 Vector3 p2;
174 Vector3 d;
175
176 float phi,t;
177
178 if ((x0 == x1) && (y0 == y1)) {
179 return Quaternion(0.0f, 0.0f, 0.0f, 1.0f); // Zero rotation
180 }
181
182
183 // Compute z coordinates for projection of p1 and p2 to
184 // deformed sphere
185 p1[0] = x0;
186 p1[1] = y0;
187 p1[2] = project_to_sphere(sphsize, x0, y0);
188
189 p2[0] = x1;
190 p2[1] = y1;
191 p2[2] = project_to_sphere(sphsize, x1, y1);
192
193
194 // Find their cross product
195 Vector3::Cross_Product(p2,p1,&a);
196
197 // Compute how much to rotate
198 d = p1 - p2;
199 t = d.Length() / (2.0f * sphsize);
200
201 // Avoid problems with out of control values
202 if (t > 1.0f) t = 1.0f;
203 if (t < -1.0f) t = -1.0f;
204 phi = 2.0f * WWMath::Asin(t);
205
206 return Axis_To_Quat(a, phi);
207}
208
209
210/***********************************************************************************************
211 * Axis_To_Quat -- Creates a quaternion given an axis and angle of rotation *
212 * *
213 * INPUT: *
214 * *
215 * OUTPUT: *
216 * *
217 * WARNINGS: *
218 * *
219 * HISTORY: *
220 * 02/28/1997 GH : Created. *
221 *=============================================================================================*/
222Quaternion Axis_To_Quat(const Vector3 &a, float phi)
223{
224 Quaternion q;
225 Vector3 tmp = a;
226
227 tmp.Normalize();
228 q[0] = tmp[0];
229 q[1] = tmp[1];
230 q[2] = tmp[2];
231
232 q.Scale(WWMath::Sin(phi / 2.0f));
233 q[3] = WWMath::Cos(phi / 2.0f);
234
235 return q;
236}
237
238/***********************************************************************************************
239 * Slerp -- Spherical Linear interpolation! *
240 * *
241 * INPUT: *
242 * p - start quaternion *
243 * q - end quaternion *
244 * alpha - interpolating parameter *
245 * *
246 * OUTPUT: *
247 * *
248 * WARNINGS: *
249 * *
250 * HISTORY: *
251 * 02/28/1997 GH : Created. *
252 *=============================================================================================*/
253
254#if 0
255#pragma warning (disable : 4725)
256
257#define ARC_TABLE_SIZE_MASK 1023
258#define SIN_TABLE_SIZE_MASK 1023
259
260void __cdecl Fast_Slerp(Quaternion& res, const Quaternion & p,const Quaternion & q,float alpha)
261{
262 float float_epsilon2=WWMATH_EPSILON * WWMATH_EPSILON;
263 float HalfOfArcTableSize=float(ARC_TABLE_SIZE/2);
264 float HalfOfSinTableSize=float(SIN_TABLE_SIZE/2);
265 const unsigned ARC_TABLE_SIZE_PER_2=ARC_TABLE_SIZE/2;
266
267 float beta; // complementary interploation parameter
268 float theta; // angle between p and q
269
270 __asm {
271 mov esi, p
272 mov edi, q
273 fld1 // we'll need 1.0 and 0.0 later
274
275// ----------------------------------------------------------------------------
276// cos theta = dot product of p and q
277// cos_t = p.X * q.X + p.Y * q.Y + p.Z * q.Z + p.W * q.W;
278// if q is on opposite hemisphere from A, use -B instead
279// if (cos_t < 0.0) {
280// cos_t = -cos_t;
281// qflip = true;
282// }
283// else {
284// qflip = false;
285// }
286// ----------------------------------------------------------------------------
287
288 fld dword ptr [esi] // p.X
289 fmul dword ptr [edi] // p.X*q.X
290 fld dword ptr [esi+08h] // p.Y
291 fmul dword ptr [edi+08h] // p.Y*q.Y
292 fld dword ptr [esi+04h] // p.Z
293 fmul dword ptr [edi+04h] // p.Z*q.Z
294 fld dword ptr [edi+0ch] // p.W
295 fmul dword ptr [esi+0ch] // p.W*q.W
296 faddp st(2), st(0) // y+=w
297 faddp st(2), st(0) // x+=z
298 faddp st(1),st(0) // x+z + y+w
299 fst beta
300 fabs
301 mov ebx,beta
302 and ebx,0x80000000
303
304// ----------------------------------------------------------------------------
305// if q is very close to p, just linearly interpolate
306// between the two.
307// if (1.0 - cos_t < WWMATH_EPSILON * WWMATH_EPSILON) {
308// beta = 1.0 - alpha;
309// }
310// ----------------------------------------------------------------------------
311
312 fld st(0) // duplicate st(0), which contains cos_t
313 fsubr st(0),st(2) // st(2) contains 1.0
314 fcomp float_epsilon2
315 fnstsw ax
316 test ah, 01h
317 je normal_slerp
318
319 fld alpha
320 fsubr st(0),st(1) // st(1) contains 1.0
321 fstp beta
322 jmp done_slerp
323
324normal_slerp:
325// ----------------------------------------------------------------------------
326// normal slerp!
327// else {
328// theta = WWMath::Acos(cos_t);
329// sin_t = WWMath::Sin(theta);
330// oo_sin_t = 1.0 / sin_t;
331// beta = WWMath::Sin(theta - alpha*theta) * oo_sin_t;
332// alpha = WWMath::Sin(alpha*theta) * oo_sin_t;
333// }
334// if (qflip) {
335// alpha = -alpha;
336// }
337// ----------------------------------------------------------------------------
338
339 fld HalfOfSinTableSize
340 fld HalfOfArcTableSize
341 fmul st(0),st(2) // cos_t * (ARC_TABLE_SIZE/2)
342 fistp theta // convert to integer
343 mov eax,theta
344 add eax,ARC_TABLE_SIZE_PER_2
345 jns no_neg
346 xor eax,eax
347 jmp contin
348no_neg:
349 cmp eax,ARC_TABLE_SIZE
350 jl contin // Note: Use Setcc/Movcc here!!!
351 mov eax,ARC_TABLE_SIZE_MASK
352contin:
353 fld dword ptr[_FastAcosTable+eax*4]
354 fst theta
355
356 fmul st(0),st(1) // theta * (sin_table_size/2)
357 fadd st(0),st(1) // theta * (sin_table_size/2) + (sin_table_size/2)
358 fistp beta // conver to integer
359 mov ecx,SIN_TABLE_SIZE_MASK
360 mov eax,beta
361 and eax,ecx // & SIN_TABLE_SIZE_MASK
362 fld dword ptr[_FastInvSinTable+eax*4] // 1.0f/sin(theta)
363
364 fld theta
365 fmul alpha // theta*alpha
366 fld st(0) // duplicate stack head as we need theta*alpha later
367 fsubr theta // theta-theta*alpha
368
369 fmul st(0),st(3) // (theta-theta*alpha)*HalfOfSinTableSize
370 fadd st(0),st(3) // (theta-theta*alpha)*HalfOfSinTableSize+HalfOfSinTableSize
371 fistp beta // convert to integer
372 mov eax,beta
373 and eax,ecx // & SIN_TABLE_SIZE_MASK
374 fld dword ptr[_FastSinTable+eax*4] // sin(theta-theta*alpha)
375
376 fmul st(0),st(2) // sin(theta-theta*alpha) * oo_sin_t
377 fstp beta
378
379 fmul st(0),st(2) // (theta*alpha)*HalfOfSinTableSize
380 fadd st(0),st(2) // (theta*alpha)*HalfOfSinTableSize+HalfOfSinTableSize
381 fistp theta // convert to integer
382 mov eax,theta
383 and eax,ecx // & SIN_TABLE_SIZE_MASK
384 fld dword ptr[_FastSinTable+eax*4] // sin(theta*alpha)
385
386 fmul st(0),st(1) // oo_sin_t
387 fstp alpha
388 fstp st(0) // pop oo_sin_t
389 fstp st(0) // pop HalfOfSinTableSize
390done_slerp:
391 test ebx, ebx
392 je no_negative
393 fld alpha
394 fchs
395 fstp alpha
396
397no_negative:
398// ----------------------------------------------------------------------------
399// res.X = beta*p.X + alpha*q.X;
400// res.Y = beta*p.Y + alpha*q.Y;
401// res.Z = beta*p.Z + alpha*q.Z;
402// res.W = beta*p.W + alpha*q.W;
403// ----------------------------------------------------------------------------
404
405 fstp st(0) // pop cos_t
406 fstp st(0) // pop 1.0
407
408 fld alpha
409 fld dword ptr [edi+4] // q.Y
410 fmul st(0),st(1) // alpha*q.Y
411 fld dword ptr [edi+8] // q.Z
412 fmul st(0),st(2) // alpha*q.Z
413 fld dword ptr [edi+12] // q.W
414 fmul st(0),st(3) // alpha*q.W
415 fld dword ptr [edi] // q.X
416 fmulp st(4),st // alpha*q.X
417
418 fld beta
419 fld dword ptr [esi+4] // p.Y
420 fmul st(0),st(1) // beta*p.Y
421 fld dword ptr [esi+8] // p.Z
422 fmul st(0),st(2) // beta*p.Z
423 fld dword ptr [esi] // p.X
424 fmul st(0),st(3) // beta*p.X
425 fxch st(3) // move beta to top of stack
426 fmul dword ptr [esi+12] // beta*p.W
427
428 faddp st(4),st // w
429 faddp st(4),st // z
430 faddp st(4),st // y
431 faddp st(4),st // x
432
433 mov ecx, res
434 fstp [ecx+12] // w
435 fstp [ecx+8] // z
436 fstp [ecx+4] // y
437 fstp [ecx] // x
438 }
439}
440#else
441void __cdecl Fast_Slerp(Quaternion& res, const Quaternion & p,const Quaternion & q,float alpha)
442{
443 float beta; // complementary interploation parameter
444 float theta; // angle between p and q
445 float cos_t; // sine, cosine of theta
446 float oo_sin_t;
447 int qflip; // use flip of q?
448
449 // cos theta = dot product of p and q
450 cos_t = p.X * q.X + p.Y * q.Y + p.Z * q.Z + p.W * q.W;
451
452 // if q is on opposite hemisphere from A, use -B instead
453 if (cos_t < 0.0f) {
454 cos_t = -cos_t;
455 qflip = true;
456 } else {
457 qflip = false;
458 }
459
460 if (1.0f - cos_t < WWMATH_EPSILON * WWMATH_EPSILON) {
461
462 // if q is very close to p, just linearly interpolate
463 // between the two.
464 beta = 1.0f - alpha;
465
466 } else {
467
468 theta = WWMath::Fast_Acos(cos_t);
469 float sin_t = WWMath::Fast_Sin(theta);
470 oo_sin_t = 1.0f / sin_t;
471 beta = WWMath::Fast_Sin(theta - alpha*theta) * oo_sin_t;
472 alpha = WWMath::Fast_Sin(alpha*theta) * oo_sin_t;
473 }
474
475 if (qflip) {
476 alpha = -alpha;
477 }
478
479 res.X = beta*p.X + alpha*q.X;
480 res.Y = beta*p.Y + alpha*q.Y;
481 res.Z = beta*p.Z + alpha*q.Z;
482 res.W = beta*p.W + alpha*q.W;
483}
484
485#endif // MSC_VER
486
487void Slerp(Quaternion& res, const Quaternion & p,const Quaternion & q,float alpha)
488{
489 float beta; // complementary interploation parameter
490 float theta; // angle between p and q
491 //float sin_t
492 float cos_t; // sine, cosine of theta
493 float oo_sin_t;
494 int qflip; // use flip of q?
495
496 // cos theta = dot product of p and q
497 cos_t = p.X * q.X + p.Y * q.Y + p.Z * q.Z + p.W * q.W;
498
499 // if q is on opposite hemisphere from A, use -B instead
500 if (cos_t < 0.0f) {
501 cos_t = -cos_t;
502 qflip = true;
503 } else {
504 qflip = false;
505 }
506
507 if (1.0f - cos_t < WWMATH_EPSILON * WWMATH_EPSILON) {
508
509 // if q is very close to p, just linearly interpolate
510 // between the two.
511 beta = 1.0f - alpha;
512
513 } else {
514
515 // normal slerp!
516 theta = WWMath::Acos(cos_t);
517 float sin_t = WWMath::Sin(theta);
518 oo_sin_t = 1.0f / sin_t;
519 beta = WWMath::Sin(theta - alpha*theta) * oo_sin_t;
520 alpha = WWMath::Sin(alpha*theta) * oo_sin_t;
521 }
522
523 if (qflip) {
524 alpha = -alpha;
525 }
526
527 res.X = beta*p.X + alpha*q.X;
528 res.Y = beta*p.Y + alpha*q.Y;
529 res.Z = beta*p.Z + alpha*q.Z;
530 res.W = beta*p.W + alpha*q.W;
531}
532
533/***********************************************************************************************
534 * Slerp_Setup -- Get ready to call "Cached_Slerp" *
535 * *
536 * INPUT: *
537 * *
538 * OUTPUT: *
539 * *
540 * WARNINGS: *
541 * *
542 * HISTORY: *
543 * 2/27/98 GTH : Created. *
544 *=============================================================================================*/
545void Slerp_Setup(const Quaternion & p,const Quaternion & q,SlerpInfoStruct * slerpinfo)
546{
547 float cos_t;
548
549 assert(slerpinfo != NULL);
550
551 // cos theta = dot product of p and q
552 cos_t = p.X * q.X + p.Y * q.Y + p.Z * q.Z + p.W * q.W;
553
554 // if q is on opposite hemisphere from A, use -B instead
555 if (cos_t < 0.0f) {
556 cos_t = -cos_t;
557 slerpinfo->Flip = true;
558 } else {
559 slerpinfo->Flip = false;
560 }
561
562 if (1.0f - cos_t < SLERP_EPSILON) {
563
564 slerpinfo->Linear = true;
565 slerpinfo->Theta = 0.0f;
566 slerpinfo->SinT = 0.0f;
567
568 } else {
569
570 slerpinfo->Linear = false;
571 slerpinfo->Theta = WWMath::Acos(cos_t);
572 slerpinfo->SinT = WWMath::Sin(slerpinfo->Theta);
573
574 }
575}
576
577/***********************************************************************************************
578 * Cached_Slerp -- Quaternion slerping, optimized with cached values *
579 * *
580 * INPUT: *
581 * *
582 * OUTPUT: *
583 * *
584 * WARNINGS: *
585 * *
586 * HISTORY: *
587 * 2/27/98 GTH : Created. *
588 *=============================================================================================*/
589Quaternion Cached_Slerp(const Quaternion & p,const Quaternion & q,float alpha,SlerpInfoStruct * slerpinfo)
590{
591 float beta; // complementary interploation parameter
592 float oo_sin_t;
593
594 if (slerpinfo->Linear) {
595
596 // if q is very close to p, just linearly interpolate
597 // between the two.
598 beta = 1.0f - alpha;
599
600 } else {
601
602 // normal slerp!
603 oo_sin_t = 1.0f / slerpinfo->Theta;
604 beta = WWMath::Sin(slerpinfo->Theta - alpha*slerpinfo->Theta) * oo_sin_t;
605 alpha = WWMath::Sin(alpha*slerpinfo->Theta) * oo_sin_t;
606 }
607
608 if (slerpinfo->Flip) {
609 alpha = -alpha;
610 }
611
612 Quaternion res;
613 res.X = beta*p.X + alpha*q.X;
614 res.Y = beta*p.Y + alpha*q.Y;
615 res.Z = beta*p.Z + alpha*q.Z;
616 res.W = beta*p.W + alpha*q.W;
617
618 return res;
619}
620
621void Cached_Slerp(const Quaternion & p,const Quaternion & q,float alpha,SlerpInfoStruct * slerpinfo,Quaternion * set_q)
622{
623 float beta; // complementary interploation parameter
624 float oo_sin_t;
625
626 if (slerpinfo->Linear) {
627
628 // if q is very close to p, just linearly interpolate
629 // between the two.
630 beta = 1.0f - alpha;
631
632 } else {
633
634 // normal slerp!
635 oo_sin_t = 1.0f / slerpinfo->Theta;
636 beta = WWMath::Sin(slerpinfo->Theta - alpha*slerpinfo->Theta) * oo_sin_t;
637 alpha = WWMath::Sin(alpha*slerpinfo->Theta) * oo_sin_t;
638 }
639
640 if (slerpinfo->Flip) {
641 alpha = -alpha;
642 }
643
644 set_q->X = beta*p.X + alpha*q.X;
645 set_q->Y = beta*p.Y + alpha*q.Y;
646 set_q->Z = beta*p.Z + alpha*q.Z;
647 set_q->W = beta*p.W + alpha*q.W;
648}
649
650/***********************************************************************************************
651 * Build_Quaternion -- Creates a quaternion from a Matrix *
652 * *
653 * INPUT: *
654 * *
655 * OUTPUT: *
656 * *
657 * WARNINGS: *
658 * Matrix MUST NOT have scaling! *
659 * *
660 * HISTORY: *
661 * 02/28/1997 GH : Created. *
662 *=============================================================================================*/
663Quaternion Build_Quaternion(const Matrix3D & mat)
664{
665 float tr,s;
666 int i,j,k;
667 Quaternion q;
668
669 // sum the diagonal of the rotation matrix
670 tr = mat[0][0] + mat[1][1] + mat[2][2];
671
672 if (tr > 0.0f) {
673
674 s = sqrt(tr + 1.0);
675 q[3] = s * 0.5;
676 s = 0.5 / s;
677
678 q[0] = (mat[2][1] - mat[1][2]) * s;
679 q[1] = (mat[0][2] - mat[2][0]) * s;
680 q[2] = (mat[1][0] - mat[0][1]) * s;
681
682 } else {
683
684 i=0;
685 if (mat[1][1] > mat[0][0]) i = 1;
686 if (mat[2][2] > mat[i][i]) i = 2;
687 j = _nxt[i];
688 k = _nxt[j];
689
690 s = sqrt((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);
691
692 q[i] = s * 0.5;
693 if (s != 0.0) {
694 s = 0.5 / s;
695 }
696
697 q[3] = ( mat[k][j] - mat[j][k] ) * s;
698 q[j] = ( mat[j][i] + mat[i][j] ) * s;
699 q[k] = ( mat[k][i] + mat[i][k] ) * s;
700
701 }
702
703 return q;
704}
705
706Quaternion Build_Quaternion(const Matrix3x3 & mat)
707{
708 float tr,s;
709 int i,j,k;
710 Quaternion q;
711
712 // sum the diagonal of the rotation matrix
713 tr = mat[0][0] + mat[1][1] + mat[2][2];
714
715 if (tr > 0.0) {
716
717 s = sqrt(tr + 1.0);
718 q[3] = s * 0.5;
719 s = 0.5 / s;
720
721 q[0] = (mat[2][1] - mat[1][2]) * s;
722 q[1] = (mat[0][2] - mat[2][0]) * s;
723 q[2] = (mat[1][0] - mat[0][1]) * s;
724
725 } else {
726
727 i = 0;
728 if (mat[1][1] > mat[0][0]) i = 1;
729 if (mat[2][2] > mat[i][i]) i = 2;
730
731 j = _nxt[i];
732 k = _nxt[j];
733
734 s = sqrt( (mat[i][i] - (mat[j][j]+mat[k][k])) + 1.0);
735
736 q[i] = s * 0.5;
737
738 if (s != 0.0) {
739 s = 0.5/s;
740 }
741
742 q[3] = ( mat[k][j] - mat[j][k] ) * s;
743 q[j] = ( mat[j][i] + mat[i][j] ) * s;
744 q[k] = ( mat[k][i] + mat[i][k] ) * s;
745 }
746
747 return q;
748}
749
750Quaternion Build_Quaternion(const Matrix4x4 & mat)
751{
752 float tr,s;
753 int i,j,k;
754 Quaternion q;
755
756 // sum the diagonal of the rotation matrix
757 tr = mat[0][0] + mat[1][1] + mat[2][2];
758
759 if (tr > 0.0) {
760
761 s = sqrt(tr + 1.0);
762 q[3] = s * 0.5;
763 s = 0.5 / s;
764
765 q[0] = (mat[2][1] - mat[1][2]) * s;
766 q[1] = (mat[0][2] - mat[2][0]) * s;
767 q[2] = (mat[1][0] - mat[0][1]) * s;
768
769 } else {
770
771 i = 0;
772 if (mat[1][1] > mat[0][0]) i = 1;
773 if (mat[2][2] > mat[i][i]) i = 2;
774
775 j = _nxt[i];
776 k = _nxt[j];
777
778 s = sqrt( (mat[i][i] - (mat[j][j]+mat[k][k])) + 1.0);
779
780 q[i] = s * 0.5;
781 if (s != 0.0) {
782 s = 0.5/s;
783 }
784 q[3] = ( mat[k][j] - mat[j][k] ) * s;
785 q[j] = ( mat[j][i] + mat[i][j] ) * s;
786 q[k] = ( mat[k][i] + mat[i][k] ) * s;
787 }
788
789 return q;
790}
791
792
793/***********************************************************************************************
794 * Build_Matrix -- Creates a Matrix from a Quaternion *
795 * *
796 * INPUT: *
797 * *
798 * OUTPUT: *
799 * *
800 * WARNINGS: *
801 * *
802 * HISTORY: *
803 * 02/28/1997 GH : Created. *
804 *=============================================================================================*/
805Matrix3x3 Build_Matrix3(const Quaternion & q)
806{
807 Matrix3x3 m;
808
809 m[0][0] = (float)(1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]));
810 m[0][1] = (float)(2.0 * (q[0] * q[1] - q[2] * q[3]));
811 m[0][2] = (float)(2.0 * (q[2] * q[0] + q[1] * q[3]));
812
813 m[1][0] = (float)(2.0 * (q[0] * q[1] + q[2] * q[3]));
814 m[1][1] = (float)(1.0 - 2.0f * (q[2] * q[2] + q[0] * q[0]));
815 m[1][2] = (float)(2.0 * (q[1] * q[2] - q[0] * q[3]));
816
817 m[2][0] = (float)(2.0 * (q[2] * q[0] - q[1] * q[3]));
818 m[2][1] = (float)(2.0 * (q[1] * q[2] + q[0] * q[3]));
819 m[2][2] =(float)(1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]));
820
821 return m;
822}
823
824Matrix4x4 Build_Matrix4(const Quaternion & q)
825{
826 Matrix4x4 m;
827
828 // initialize the rotation sub-matrix
829 m[0][0] = (float)(1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]));
830 m[0][1] = (float)(2.0 * (q[0] * q[1] - q[2] * q[3]));
831 m[0][2] = (float)(2.0 * (q[2] * q[0] + q[1] * q[3]));
832
833 m[1][0] = (float)(2.0 * (q[0] * q[1] + q[2] * q[3]));
834 m[1][1] = (float)(1.0 - 2.0f * (q[2] * q[2] + q[0] * q[0]));
835 m[1][2] = (float)(2.0 * (q[1] * q[2] - q[0] * q[3]));
836
837 m[2][0] = (float)(2.0 * (q[2] * q[0] - q[1] * q[3]));
838 m[2][1] = (float)(2.0 * (q[1] * q[2] + q[0] * q[3]));
839 m[2][2] = (float)(1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]));
840
841 // no translation
842 m[0][3] = m[1][3] = m[2][3] = 0.0f;
843
844 // last row
845 m[3][0] = m[3][1] = m[3][2] = 0.0f;
846 m[3][3] = 1.0f;
847 return m;
848}
849
850void Quaternion::Rotate_X(float theta)
851{
852 // TODO: optimize this
853 *this = (*this) * Quaternion(Vector3(1,0,0),theta);
854}
855
856void Quaternion::Rotate_Y(float theta)
857{
858 // TODO: optimize this
859 *this = (*this) * Quaternion(Vector3(0,1,0),theta);
860}
861
862void Quaternion::Rotate_Z(float theta)
863{
864 // TODO: optimize this
865 *this = (*this) * Quaternion(Vector3(0,0,1),theta);
866}
867
868float project_to_sphere(float r, float x, float y)
869{
870 const float SQRT2 = 1.41421356f;
871 float t, z;
872 float d = WWMath::Sqrt(x * x + y * y);
873
874 if (d < r * (SQRT2/(2.0f))) // inside sphere
875 z = WWMath::Sqrt(r * r - d * d);
876 else { // on hyperbola
877 t = r / SQRT2;
878 z = t * t / d;
879 }
880
881 return z;
882}
883
884
885void Quaternion::Randomize(void)
886{
887 X = ((float) (rand() & 0xFFFF)) / 65536.0f;
888 Y = ((float) (rand() & 0xFFFF)) / 65536.0f;
889 Z = ((float) (rand() & 0xFFFF)) / 65536.0f;
890 W = ((float) (rand() & 0xFFFF)) / 65536.0f;
891
892 Normalize();
893}
894
895
NULL
#define NULL
Definition BaseType.h:92
__cdecl
#define __cdecl
Definition IFF.H:44
_FastInvSinTable
float _FastInvSinTable[SIN_TABLE_SIZE]
Definition wwmath.cpp:49
_FastAcosTable
float _FastAcosTable[ARC_TABLE_SIZE]
Definition wwmath.cpp:46
_FastSinTable
float _FastSinTable[SIN_TABLE_SIZE]
Definition wwmath.cpp:48
WWMATH_EPSILON
#define WWMATH_EPSILON
Definition wwmath.h:54
SIN_TABLE_SIZE
const int SIN_TABLE_SIZE
Definition wwmath.h:92
ARC_TABLE_SIZE
const int ARC_TABLE_SIZE
Definition wwmath.h:91
add
void add(float *sum, float *addend)
Definition W3DShaderManager.cpp:3146
Quaternion::X
float X
Definition quat.h:60
Quaternion::Quaternion
WWINLINE Quaternion(void)
Definition quat.h:67
Quaternion::Rotate_Y
void Rotate_Y(float theta)
Definition quat.cpp:856
Quaternion::Make_Closest
Quaternion & Make_Closest(const Quaternion &qto)
Definition quat.cpp:139
Quaternion::Normalize
void Normalize(void)
Definition quat.cpp:112
Quaternion::Z
float Z
Definition quat.h:62
Quaternion::Rotate_X
void Rotate_X(float theta)
Definition quat.cpp:850
Quaternion::Y
float Y
Definition quat.h:61
Quaternion::Rotate_Z
void Rotate_Z(float theta)
Definition quat.cpp:862
Quaternion::W
float W
Definition quat.h:63
Quaternion::Randomize
void Randomize(void)
Definition quat.cpp:885
Vector3::X
float X
Definition vector3.h:90
Vector3::Z
float Z
Definition vector3.h:92
Vector3::Y
float Y
Definition vector3.h:91
Vector3::Length
WWINLINE float Length(void) const
Definition vector3.h:453
Vector3::Normalize
void Normalize(void)
Definition vector3.h:417
Vector3::Cross_Product
static WWINLINE void Cross_Product(const Vector3 &a, const Vector3 &b, Vector3 *result)
Definition vector3.h:374
WWMath::Asin
static WWINLINE float Asin(float val)
Definition wwmath.h:547
WWMath::Sqrt
static float Sqrt(float val)
Definition wwmath.h:568
WWMath::Fast_Sin
static WWINLINE float Fast_Sin(float val)
Definition wwmath.h:388
WWMath::Inv_Sqrt
static float Inv_Sqrt(float a)
Definition wwmath.h:651
WWMath::Acos
static WWINLINE float Acos(float val)
Definition wwmath.h:510
WWMath::Sin
static float Sin(float val)
Definition wwmath.h:378
WWMath::Fast_Acos
static WWINLINE float Fast_Acos(float val)
Definition wwmath.h:482
WWMath::Cos
static float Cos(float val)
Definition wwmath.h:356
q
int q
Definition test1.cpp:94
Cached_Slerp
Quaternion Cached_Slerp(const Quaternion &p, const Quaternion &q, float alpha, SlerpInfoStruct *slerpinfo)
Definition quat.cpp:589
Slerp
void Slerp(Quaternion &res, const Quaternion &p, const Quaternion &q, float alpha)
Definition quat.cpp:487
Slerp_Setup
void Slerp_Setup(const Quaternion &p, const Quaternion &q, SlerpInfoStruct *slerpinfo)
Definition quat.cpp:545
SLERP_EPSILON
#define SLERP_EPSILON
Definition quat.cpp:62
Build_Quaternion
Quaternion Build_Quaternion(const Matrix3D &mat)
Definition quat.cpp:663
Build_Matrix4
Matrix4x4 Build_Matrix4(const Quaternion &q)
Definition quat.cpp:824
Trackball
Quaternion Trackball(float x0, float y0, float x1, float y1, float sphsize)
Definition quat.cpp:169
Axis_To_Quat
Quaternion Axis_To_Quat(const Vector3 &a, float phi)
Definition quat.cpp:222
Build_Matrix3
Matrix3x3 Build_Matrix3(const Quaternion &q)
Definition quat.cpp:805
Fast_Slerp
void __cdecl Fast_Slerp(Quaternion &res, const Quaternion &p, const Quaternion &q, float alpha)
Definition quat.cpp:441
SlerpInfoStruct::Theta
float Theta
Definition quat.h:236
SlerpInfoStruct::Linear
bool Linear
Definition quat.h:238
SlerpInfoStruct::SinT
float SinT
Definition quat.h:235
test
int test
Definition test6.cpp:32