来自@Golden_Shadow 我的3D引擎数学库实现文件

1. #include "stdafx.h"  
2. #include <math.h>  
3. #include "MyMath.h"  
4.   
5. float FastSin(float theta)  
6. {  
7.     theta = fmodf(theta, 360) ;  
8.   
9.     if (theta < 0)  
10.         theta += 360 ;  
11.   
12.     int thetaIntPart = (int)theta ;  
13.     float thetaFracPart = theta - thetaIntPart ;  
14.   
15.     return sinLookUpTable[thetaIntPart] +  
16.         thetaFracPart * (sinLookUpTable[thetaIntPart + 1] - sinLookUpTable[thetaIntPart]) ;  
17. }  
18.   
19. float FastCos(float theta)  
20. {  
21.     theta = fmodf(theta, 360) ;  
22.   
23.     if (theta < 0)  
24.         theta += 360 ;  
25.   
26.     int thetaIntPart = (int)theta ;  
27.     float thetaFracPart = theta - thetaIntPart ;  
28.   
29.     return cosLookUpTable[thetaIntPart] +  
30.         thetaFracPart * (cosLookUpTable[thetaIntPart + 1] - cosLookUpTable[thetaIntPart]) ;  
31. }  
32.   
33. int Fast_Distance_2D(int x, int y)  
34. {  
35.     x = abs(x) ;  
36.     y = abs(y) ;  
37.   
38.     int mn = MIN(x, y) ;  
39.       
40.     return (x + y - (mn >> 1) - (mn >> 2) + (mn >> 4)) ;  
41. }  
42.   
43.   
44. float Fast_Distance_3D(float x, float y, float z)  
45. {  
46.     int temp ;  
47.     int newX, newY, newZ ;  
48.   
49.     newX = fabs(x) * 1024 ;  
50.     newY = fabs(y) * 1024 ;  
51.     newZ = fabs(z) * 1024 ;  
52.   
53.     if (newY < newX)  
54.         SWAP(newX, newY, temp) ;  
55.     if (newZ < newY)  
56.         SWAP(newZ, newY, temp) ;  
57.     if (newY < newX)  
58.         SWAP(newX, newY, temp) ;  
59.   
60.     int distance = (newZ + 11 * (newY >> 5) + (newX >> 2)) ;  
61.   
62.     return ((float)(distance >> 10)) ;  
63. }  
64.   
65.   
66. void POLAR2D_To_POINT2D(POLAR2D_PTR polar, POINT2D_PTR rect)  
67. {  
68.     rect ->y = FastSin(polar ->theta) * polar ->r ;  
69.     rect ->x = FastCos(polar ->theta) * polar ->r ;  
70. }  
71.   
72. void POINT2D_To_POLAR2D(POINT2D_PTR rect , POLAR2D_PTR polar)  
73. {  
74.     polar ->theta = atanf(rect ->y / rect ->x) ;  
75.     polar ->r = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y)) ;  
76. }  
77.   
78. void POINT2D_To_PolarRTh(POINT2D_PTR rect, float * pR, float * pTheta)  
79. {  
80.     *pTheta = atanf(rect ->y / rect ->x) ;  
81.     *pR = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y)) ;  
82. }  
83.   
84. void CYLINDRICAL3D_To_POINT3D(CYLINDRICAL3D_PTR cyl, POINT3D_PTR rect)  
85. {  
86.     rect ->x = FastCos(cyl ->theta) * cyl ->r ;  
87.     rect ->y = FastSin(cyl ->theta) * cyl ->r ;  
88.     rect ->z = cyl ->z ;  
89. }  
90.   
91. void CYLINDRICAL3D_To_POINT3D(CYLINDRICAL3D_PTR cyl, float *pX, float *pY, float *pZ)  
92. {  
93.     *pX = FastCos(cyl ->theta) * cyl ->r ;  
94.     *pY = FastSin(cyl ->theta) * cyl ->r ;  
95.     *pZ = cyl ->z ;  
96. }  
97.   
98. void POINT3D_To_CYLINDRICAL3D(POINT3D_PTR rect, CYLINDRICAL3D_PTR cyl)  
99. {  
100.     cyl ->theta = atanf(rect ->y / rect ->x) ;  
101.     cyl ->r = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y)) ;  
102.     cyl ->z = rect ->z ;  
103. }  
104.   
105. void SPHERICAL3D_To_POINT3D(SPHERICAL3D_PTR sph, POINT3D_PTR rect)  
106. {  
107.     float r = sph ->p * FastSin(sph ->theta) ;  
108.     rect ->x = r * FastCos(sph ->phi) ;  
109.     rect ->y = r * FastSin(sph ->phi) ;  
110.     rect ->z = FastCos(sph ->theta) * sph ->p ;  
111. }  
112.   
113. void SPHERICAL3D_To_RectXYZ(SPHERICAL3D_PTR sph, float * pX, float * pY, float * pZ)  
114. {  
115.     float r = sph ->p * FastSin(sph ->theta) ;  
116.     *pX = r * FastCos(sph ->phi) ;  
117.     *pY = r * FastSin(sph ->phi) ;  
118.     *pZ= FastCos(sph ->theta) * sph ->p ;  
119. }  
120.   
121. void POINT3D_To_SPHERICAL3D(POINT3D_PTR rect, SPHERICAL3D_PTR sph)  
122. {  
123.     sph ->p = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y) + (rect ->z * rect ->z)) ;  
124.     sph ->phi = atanf(rect ->y / rect ->x) ;  
125.     float r = rect ->y / FastSin(sph ->phi) ;  
126.     sph ->theta = asinf(r / sph ->p) ;  
127. }  
128.   
129. void POINT3D_To_SphericalPThPh(POINT3D_PTR rect, float * pP, float * pTheta, float * pPhi)  
130. {  
131.     *pP = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y) + (rect ->z * rect ->z)) ;  
132.     *pPhi = atanf(rect ->y / rect ->x) ;  
133.     float r = rect ->y / FastSin(*pPhi) ;  
134.     *pTheta = asinf(r / *pP) ;  
135. }  
136.   
137. void VECTOR2D_Add(VECTOR2D_PTR va, VECTOR2D_PTR vb, VECTOR2D_PTR vsum)  
138. {  
139.     vsum ->x = va ->x + vb ->x ;  
140.     vsum ->y = va ->y + vb ->y ;  
141. }  
142.   
143. VECTOR2D VECTOR2D_Add(VECTOR2D_PTR va, VECTOR2D_PTR vb)  
144. {  
145.     VECTOR2D result ;  
146.     result.x = va ->x + vb ->y ;  
147.     result.y = va ->y + vb ->y ;  
148.   
149.     return result ;  
150. }  
151.   
152. void VECTOR2D_Sub(VECTOR2D_PTR va, VECTOR2D_PTR vb, VECTOR2D_PTR vsum)  
153. {  
154.     vsum ->x = va ->x - vb ->x ;  
155.     vsum ->y = va ->y - vb ->y ;  
156. }  
157.   
158. VECTOR2D VECTOR2D_Sub(VECTOR2D_PTR va, VECTOR2D_PTR vb)  
159. {  
160.     VECTOR2D result ;  
161.     result.x = va ->x - vb ->x ;  
162.     result.y = va ->y - vb ->y ;  
163.   
164.     return result ;  
165. }  
166.   
167. void VECTOR3D_Add(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vsum)  
168. {  
169.     vsum ->x = va ->x + vb ->x ;  
170.     vsum ->y = va ->y + vb ->y ;  
171.     vsum ->z = va ->z + vb ->z ;  
172. }  
173.   
174. VECTOR3D VECTOR3D_Add(VECTOR3D_PTR va, VECTOR3D_PTR vb)  
175. {  
176.     VECTOR3D result ;  
177.     result.x = va ->x + vb ->x ;  
178.     result.y = va ->y + vb ->y ;  
179.     result.z = va ->z + vb ->z ;  
180.   
181.     return result ;  
182. }  
183.   
184. void VECTOR3D_Sub(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vdiff)  
185. {  
186.     vdiff ->x = va ->x - vb ->x ;  
187.     vdiff ->y = va ->y - vb ->y ;  
188.     vdiff ->z = va ->z - vb ->z ;  
189. }  
190.   
191. VECTOR3D VECTOR3D_Sub(VECTOR3D_PTR va, VECTOR3D_PTR vb)  
192. {  
193.     VECTOR3D result ;  
194.     result.x = va ->x - vb ->x ;  
195.     result.y = va ->y - vb ->y ;  
196.     result.z = va ->z - vb ->z ;  
197.   
198.     return result ;  
199. }  
200.   
201. //      4D向量加法  
202. void VECTOR4D_Add(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vsum)  
203. {  
204.     vsum ->x = va ->x + vb ->x ;  
205.     vsum ->y = va ->y + vb ->y ;  
206.     vsum ->z = va ->z + vb ->z ;  
207.     vsum ->w = 1 ;  
208. }  
209.   
210. VECTOR4D VECTOR4D_Add(VECTOR4D_PTR va, VECTOR4D_PTR vb)  
211. {  
212.     VECTOR4D result ;  
213.     result.x = va ->x + vb ->x ;  
214.     result.y = va ->y + vb ->y ;  
215.     result.z = va ->z + vb ->z ;  
216.     result.w = 1 ;  
217.   
218.     return result ;  
219. }  
220.   
221. //      4D向量减法  
222. void VECTOR4D_Sub(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vdiff)  
223. {  
224.     vdiff ->x = va ->x - vb ->x ;  
225.     vdiff ->y = va ->y - vb ->y ;  
226.     vdiff ->z = va ->z - vb ->z ;  
227.     vdiff ->w = 1 ;  
228. }  
229.   
230. VECTOR4D VECTOR4D_Sub(VECTOR4D_PTR va, VECTOR4D_PTR vb)  
231. {  
232.     VECTOR4D result ;  
233.     result.x = va ->x - vb ->x ;  
234.     result.y = va ->y - vb ->y ;  
235.     result.z = va ->z - vb ->z ;  
236.     result.w = 1 ;  
237.   
238.     return result ;  
239. }  
240.   
241. void VECTOR2D_Scale(float k, VECTOR2D_PTR va, VECTOR2D_PTR vscaled)  
242. {  
243.     vscaled ->x = va ->x * k ;  
244.     vscaled ->y = va ->y * k ;  
245. }  
246.   
247. void VECTOR2D_Scale(float k, VECTOR2D_PTR va)  
248. {  
249.     va ->x *= k ;  
250.     va ->y *= k ;  
251. }  
252.   
253. void VECTOR3D_Scale(float k, VECTOR3D_PTR va, VECTOR3D_PTR vscaled)  
254. {  
255.     vscaled ->x = va ->x * k ;  
256.     vscaled ->y = va ->y * k ;  
257.     vscaled ->z = va ->z * k ;  
258. }  
259.   
260. void VECTOR3D_Scale(float k, VECTOR3D_PTR va)  
261. {  
262.     va ->x *= k ;  
263.     va ->y *= k ;  
264.     va ->z *= k ;  
265. }  
266.   
267. void VECTOR4D_Scale(float k, VECTOR4D_PTR va, VECTOR4D_PTR vscaled)  
268. {  
269.     vscaled ->x = va ->x * k ;  
270.     vscaled ->y = va ->y * k ;  
271.     vscaled ->z = va ->z * k ;  
272.     vscaled ->w = 1 ;  
273. }  
274.   
275. void VECTOR4D_Scale(float k, VECTOR4D_PTR va)  
276. {  
277.     va ->x *= k ;  
278.     va ->y *= k ;  
279.     va ->z *= k ;  
280.     va ->w = 1 ;  
281. }  
282.   
283. float VECTOR2D_Dot(VECTOR2D_PTR va, VECTOR2D_PTR vb)  
284. {  
285.     return va ->x * vb ->x + va ->y * vb ->y ;  
286. }  
287.   
288. float VECTOR3D_Dot(VECTOR3D_PTR va, VECTOR3D_PTR vb)  
289. {  
290.     return va ->x * vb ->x + va ->y * vb ->y + va ->z * vb ->z ;  
291. }  
292.   
293. float VECTOR4D_Dot(VECTOR4D_PTR va, VECTOR4D_PTR vb)  
294. {  
295.     return va ->x * vb ->x + va ->y * vb ->y + va ->z * vb ->z ;  
296. }  
297.   
298. void VECTOR3D_Cross(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vn)  
299. {  
300.     vn ->x = va ->y * vb ->z - va ->z * vb ->y ;  
301.     vn ->y = va ->z * vb ->x - va ->x * vb ->z ;  
302.     vn ->z = va ->x * vb ->y - va ->y * vb ->x ;  
303. }  
304.   
305. VECTOR3D VECTOR3D_Cross(VECTOR3D_PTR va, VECTOR3D_PTR vb)  
306. {  
307.     VECTOR3D result ;  
308.     result.x = va ->y * vb ->z - va ->z * vb ->y ;  
309.     result.y = va ->z * vb ->x - va ->x * vb ->z ;  
310.     result.z = va ->x * vb ->y - va ->y * vb ->x ;  
311.   
312.     return result ;  
313. }  
314.   
315. void VECTOR4D_Cross(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vn)  
316. {  
317.     vn ->x = va ->y * vb ->z - va ->z * vb ->y ;  
318.     vn ->y = va ->z * vb ->x - va ->x * vb ->z ;  
319.     vn ->z = va ->x * vb ->y - va ->y * vb ->x ;  
320.     vn ->w = 1 ;  
321. }  
322.   
323. VECTOR4D VECTOR4D_Cross(VECTOR4D_PTR va, VECTOR4D_PTR vb)  
324. {  
325.     VECTOR4D result ;  
326.     result.x = va ->y * vb ->z - va ->z * vb ->y ;  
327.     result.y = va ->z * vb ->x - va ->x * vb ->z ;  
328.     result.z = va ->x * vb ->y - va ->y * vb ->x ;  
329.     result.w = 1 ;  
330.   
331.     return result ;  
332. }  
333.   
334. float VECTOR2D_Length(VECTOR2D_PTR va)  
335. {  
336.     return sqrtf(va ->x * va ->x + va ->y * va ->y) ;  
337. }  
338.   
339. float VECTOR3D_Length(VECTOR3D_PTR va)  
340. {  
341.     return sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;  
342. }  
343.   
344. float VECTOR4D_Length(VECTOR4D_PTR va)  
345. {  
346.     return sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;  
347. }  
348.   
349. float VECTOR2D_Length_Fast(VECTOR2D_PTR va)  
350. {  
351.     return ((float) Fast_Distance_2D(va ->x, va ->y)) ;  
352. }  
353.   
354. float VECTOR3D_Length_Fast(VECTOR3D_PTR va)  
355. {  
356.     return (Fast_Distance_3D(va ->x, va ->y, va ->z)) ;  
357. }  
358.   
359. float VECTOR4D_Length_Fast(VECTOR4D_PTR va)  
360. {  
361.     return (Fast_Distance_3D(va ->x, va ->y, va ->z)) ;  
362. }  
363.   
364. void VECTOR2D_Normalize(VECTOR2D_PTR va)  
365. {  
366.     float length = sqrtf(va ->x * va ->x + va ->y * va ->y) ;  
367.   
368.     if (length < EPSILON_E5)  
369.         return ;  
370.   
371.     va ->x /= length ;  
372.     va ->y /= length ;  
373. }  
374.   
375.   
376. void VECTOR3D_Normalize(VECTOR3D_PTR va)  
377. {  
378.     float length = sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;  
379.   
380.     if (length < EPSILON_E5)  
381.         return ;  
382.   
383.     va ->x /= length ;  
384.     va ->y /= length ;  
385.     va ->z /= length ;  
386. }  
387.   
388. void VECTOR4D_Normalize(VECTOR4D_PTR va)  
389. {  
390.     float length = sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;  
391.   
392.     if (length < EPSILON_E5)  
393.         return ;  
394.   
395.     va ->x /= length ;  
396.     va ->y /= length ;  
397.     va ->z /= length ;  
398.     va ->w = 1 ;  
399. }  
400.   
401. void VECTOR2D_Normalize(VECTOR2D_PTR va, VECTOR2D_PTR vn)  
402. {  
403.     float length = sqrtf(va ->x * va ->x + va ->y * va ->y) ;  
404.   
405.     if (length < EPSILON_E5)  
406.         return ;  
407.   
408.     vn ->x = va ->x / length ;  
409.     vn ->y = va ->y / length ;  
410. }  
411.   
412. void VECTOR3D_Normalize(VECTOR3D_PTR va, VECTOR3D_PTR vn)  
413. {  
414.     float length = sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;  
415.   
416.     if (length < EPSILON_E5)  
417.         return ;  
418.   
419.     vn ->x = va ->x / length ;  
420.     vn ->y = va ->y / length ;  
421.     vn ->z = va ->z / length ;  
422. }  
423.   
424. void VECTOR4D_Normalize(VECTOR4D_PTR va, VECTOR4D_PTR vn)  
425. {  
426.     float length = sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;  
427.   
428.     if (length < EPSILON_E5)  
429.         return ;  
430.   
431.     vn ->x = va ->x / length ;  
432.     vn ->y = va ->y / length ;  
433.     vn ->z = va ->z / length ;  
434.     va ->w = 1 ;  
435. }  
436.   
437. void VECTOR2D_Build(VECTOR2D_PTR init, VECTOR2D_PTR term, VECTOR2D_PTR result)  
438. {  
439.     result ->x = term ->x - init ->x ;  
440.     result ->y = term ->y - init ->y ;  
441. }  
442.   
443. void VECTOR3D_Build(VECTOR3D_PTR init, VECTOR3D_PTR term, VECTOR3D_PTR result)  
444. {  
445.     result ->x = term ->x - init ->x ;  
446.     result ->y = term ->y - init ->y ;  
447.     result ->z = term ->z - init ->z ;  
448. }  
449.   
450. void VECTOR4D_Build(VECTOR4D_PTR init, VECTOR4D_PTR term, VECTOR4D_PTR result)  
451. {  
452.     result ->x = term ->x - init ->x ;  
453.     result ->y = term ->y - init ->y ;  
454.     result ->z = term ->z - init ->z ;  
455.     result ->w = 1 ;  
456. }  
457.   
458. float VECTOR2D_CosTh(VECTOR2D_PTR va, VECTOR2D_PTR vb)  
459. {  
460.     return (VECTOR2D_Dot(va, vb) / (VECTOR2D_Length(va) * VECTOR2D_Length(vb))) ;  
461. }  
462.   
463. float VECTOR3D_CosTh(VECTOR3D_PTR va, VECTOR3D_PTR vb)  
464. {  
465.     return (VECTOR3D_Dot(va, vb) / (VECTOR3D_Length(va) * VECTOR3D_Length(vb))) ;  
466. }  
467.   
468. float VECTOR4D_CosTh(VECTOR4D_PTR va, VECTOR4D_PTR vb)  
469. {  
470.     return (VECTOR4D_Dot(va, vb) / (VECTOR4D_Length(va) * VECTOR4D_Length(vb))) ;  
471. }  
472.   
473. void Mat_Init_2X2(MATRIX2X2_PTR ma, float m00, float m01, float m10, float m11)  
474. {  
475.     ma ->M00 = m00 ;  
476.     ma ->M01 = m01 ;  
477.     ma ->M10 = m10 ;   
478.     ma ->M11 = m11 ;  
479. }  
480.   
481. void Mat_Add_2X2(MATRIX2X2_PTR ma, MATRIX2X2_PTR mb, MATRIX2X2_PTR msum)  
482. {  
483.     msum ->M00 = ma ->M00 + mb ->M00 ;  
484.     msum ->M01 = ma ->M01 + mb ->M01 ;  
485.     msum ->M10 = ma ->M10 + mb ->M10 ;  
486.     msum ->M11 = ma ->M11 + mb ->M11 ;  
487. }  
488.   
489. void Mat_Mul_2X2(MATRIX2X2_PTR ma, MATRIX2X2_PTR mb, MATRIX2X2_PTR mprod)  
490. {  
491.     mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 ;  
492.     mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 ;  
493.     mprod ->M10 = ma ->M10 * mb ->M00 + ma ->M11 * mb ->M10 ;  
494.     mprod ->M11 = ma ->M10 * mb ->M01 + ma ->M11 * mb ->M11 ;  
495. }  
496.   
497. void Mat_Mul_1X2_3X2(MATRIX1X2_PTR ma, MATRIX3X2_PTR mb, MATRIX1X2_PTR mprod)  
498. {  
499.     mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + 1 * mb ->M20 ;  
500.     mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + 1 * mb ->M21 ;  
501. }  
502.   
503. float Mat_Det_2X2(MATRIX2X2_PTR m)  
504. {  
505.     return m ->M00 * m ->M11 - m ->M01 * m ->M10 ;  
506. }  
507.   
508. bool Mat_Inverse_2X2(MATRIX2X2_PTR m, MATRIX2X2_PTR mi)  
509. {  
510.     float det = Mat_Det_2X2(m) ;  
511.       
512.     if (fabs(det) < EPSILON_E5)  
513.         return false ;  
514.   
515.     mi ->M00 = m ->M11 / det ;  
516.     mi ->M01 = -(m ->M01 / det) ;  
517.     mi ->M10 = -(m ->M10 / det) ;  
518.     mi ->M11 = m ->M00 / det ;  
519.   
520.     return true ;  
521. }  
522.   
523. void Mat_Init_3X3(MATRIX3X3_PTR ma,  
524.     float m00, float m01, float m02,  
525.     float m10, float m11, float m12,  
526.     float m20, float m21, float m22)  
527. {  
528.     ma ->M00 = m00 ; ma ->M01 = m01 ; ma ->M02 = m02 ;  
529.     ma ->M10 = m10 ; ma ->M11 = m11 ; ma ->M12 = m12 ;  
530.     ma ->M20 = m20 ; ma ->M21 = m21 ; ma ->M22 = m22 ;  
531. }  
532.   
533. void Mat_Add_3X3(MATRIX3X3_PTR ma, MATRIX3X3_PTR mb, MATRIX3X3_PTR msum)  
534. {  
535.     msum ->M00 = ma ->M00 + mb ->M00 ;  
536.     msum ->M01 = ma ->M01 + mb ->M01 ;  
537.     msum ->M02 = ma ->M02 + mb ->M02 ;  
538.     msum ->M10 = ma ->M10 + mb ->M10 ;  
539.     msum ->M11 = ma ->M11 + mb ->M11 ;  
540.     msum ->M12 = ma ->M12 + mb ->M12 ;  
541.     msum ->M20 = ma ->M20 + mb ->M20 ;  
542.     msum ->M21 = ma ->M21 + mb ->M21 ;  
543.     msum ->M22 = ma ->M22 + mb ->M22 ;  
544. }  
545.   
546. void Mat_Mul_3X3_3X3(MATRIX3X3_PTR ma, MATRIX3X3_PTR mb, MATRIX3X3_PTR mprod)  
547. {  
548.     mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + ma ->M02 * mb ->M20 ;  
549.     mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + ma ->M02 * mb ->M21 ;  
550.     mprod ->M02 = ma ->M00 * mb ->M02 + ma ->M01 * mb ->M12 + ma ->M02 * mb ->M22 ;  
551.     mprod ->M10 = ma ->M10 * mb ->M00 + ma ->M11 * mb ->M10 + ma ->M12 * mb ->M20 ;  
552.     mprod ->M11 = ma ->M10 * mb ->M01 + ma ->M11 * mb ->M11 + ma ->M12 * mb ->M21 ;  
553.     mprod ->M12 = ma ->M10 * mb ->M02 + ma ->M11 * mb ->M12 + ma ->M12 * mb ->M22 ;  
554.     mprod ->M20 = ma ->M20 * mb ->M00 + ma ->M21 * mb ->M10 + ma ->M22 * mb ->M20 ;  
555.     mprod ->M21 = ma ->M20 * mb ->M01 + ma ->M21 * mb ->M11 + ma ->M22 * mb ->M21 ;  
556.     mprod ->M22 = ma ->M20 * mb ->M02 + ma ->M21 * mb ->M12 + ma ->M22 * mb ->M22 ;  
557. }  
558.   
559. float Mat_Det_3X3(MATRIX3X3_PTR m)  
560. {  
561.     return m ->M00 * (m ->M11 * m ->M22 - m ->M21 * m ->M12) -  
562.         m ->M01 * (m ->M10 * m ->M22 - m ->M20 * m ->M12) +  
563.         m ->M02 * (m ->M10 * m ->M21 - m ->M20 * m ->M11) ;  
564. }  
565.   
566. bool Mat_Inverse_3X3(MATRIX3X3_PTR m, MATRIX3X3_PTR mi)  
567. {  
568.     float det = Mat_Det_3X3(m) ;  
569.   
570.     if (fabs(det) < EPSILON_E5)  
571.         return false ;  
572.   
573.     mi ->M00 = (m ->M11 * m ->M22 - m ->M21 * m ->M12) / det ;  
574.     mi ->M10 = -(m ->M10 * m ->M22 - m ->M20 * m ->M12) / det ;  
575.     mi ->M20 = (m ->M10 * m ->M21 - m ->M20 * m ->M11) / det ;  
576.   
577.     mi ->M01 = -(m ->M01 * m ->M22 - m ->M21 * m ->M02) / det ;  
578.     mi ->M11 = (m ->M00 * m ->M22 - m ->M20 * m ->M02) / det ;  
579.     mi ->M21 = -(m ->M00 * m ->M21 - m ->M20 * m ->M01) / det ;  
580.   
581.     mi ->M02 = (m ->M01 * m ->M12 - m ->M11 * m ->M02) / det ;  
582.     mi ->M12 = -(m ->M00 * m ->M12 - m ->M10 * m ->M02) / det ;  
583.     mi ->M22 = (m ->M00 * m ->M11 - m ->M10 * m ->M01) / det ;  
584.   
585.     return true ;  
586. }  
587.   
588. void Mat_Mul_VECTOT3D_3X3(VECTOR3D_PTR va, MATRIX3X3_PTR mb, VECTOR3D_PTR vprod)  
589. {  
590.     vprod ->x = va ->x * mb ->M00 + va ->y * mb ->M10 + va ->z * mb ->M20 ;  
591.     vprod ->y = va ->x * mb ->M01 + va ->y * mb ->M11 + va ->z * mb ->M21 ;  
592.     vprod ->z = va ->x * mb ->M02 + va ->y * mb ->M12 + va ->z * mb ->M22 ;  
593. }  
594.   
595. void Mat_Mul_1X3_3X3(MATRIX1X3_PTR ma, MATRIX3X3_PTR mb, MATRIX1X3_PTR mprod)  
596. {  
597.     mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + ma ->M02 * mb ->M20 ;  
598.     mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + ma ->M02 * mb ->M21 ;  
599.     mprod ->M02 = ma ->M00 * mb ->M11 + ma ->M01 * mb ->M21 + ma ->M02 * mb ->M22 ;  
600. }  
601.   
602. void Mat_Init_4X4(MATRIX4X4_PTR ma,  
603.     float m00, float m01, float m02, float m03,  
604.     float m10, float m11, float m12, float m13,  
605.     float m20, float m21, float m22, float m23,  
606.     float m30, float m31, float m32, float m33)  
607. {  
608.     ma ->M00 = m00 ; ma ->M01 = m01 ; ma ->M02 = m02 ; ma ->M03 = m03 ;  
609.     ma ->M10 = m10 ; ma ->M11 = m11 ; ma ->M12 = m12 ; ma ->M13 = m13 ;  
610.     ma ->M20 = m20 ; ma ->M21 = m21 ; ma ->M22 = m22 ; ma ->M23 = m23 ;  
611.     ma ->M30 = m30 ; ma ->M31 = m31 ; ma ->M32 = m32 ; ma ->M33 = m33 ;  
612. }  
613.   
614. void Mat_Add_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR msum)  
615. {  
616.     msum ->M00 = ma ->M00 + mb ->M00 ;  
617.     msum ->M01 = ma ->M01 + mb ->M01 ;  
618.     msum ->M02 = ma ->M02 + mb ->M02 ;  
619.     msum ->M03 = ma ->M03 + mb ->M03 ;  
620.     msum ->M10 = ma ->M10 + mb ->M10 ;  
621.     msum ->M11 = ma ->M11 + mb ->M11 ;  
622.     msum ->M12 = ma ->M12 + mb ->M12 ;  
623.     msum ->M13 = ma ->M13 + mb ->M13 ;  
624.     msum ->M20 = ma ->M20 + mb ->M20 ;  
625.     msum ->M21 = ma ->M21 + mb ->M21 ;  
626.     msum ->M22 = ma ->M22 + mb ->M22 ;  
627.     msum ->M33 = ma ->M33 + mb ->M33 ;  
628. }  
629.   
630. void Mat_Mul_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR mprod)  
631. {  
632.     //for (int row = 0; row < 4; ++row)  
633.     //{  
634.     //  for (int col = 0; col < 4; ++col)  
635.     //  {  
636.     //      float sum = 0 ;  
637.   
638.     //      for (int index = 0; index < 4; ++index)  
639.     //      {  
640.     //          sum += ma ->M[row][index] * mb ->M[index][col] ;  
641.     //      }  
642.   
643.     //      mprod ->M[row][col] = sum ;  
644.     //  }  
645.     //}  
646.   
647.     mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + ma ->M02 * mb ->M20 + ma ->M03 * mb ->M30 ;  
648.     mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + ma ->M02 * mb ->M21 + ma ->M03 * mb ->M31 ;  
649.     mprod ->M02 = ma ->M00 * mb ->M02 + ma ->M01 * mb ->M12 + ma ->M02 * mb ->M22 + ma ->M03 * mb ->M32 ;  
650.     mprod ->M03 = ma ->M00 * mb ->M03 + ma ->M01 * mb ->M13 + ma ->M02 * mb ->M23 + ma ->M03 * mb ->M33 ;  
651.     mprod ->M10 = ma ->M10 * mb ->M00 + ma ->M11 * mb ->M10 + ma ->M12 * mb ->M20 + ma ->M13 * mb ->M30 ;  
652.     mprod ->M11 = ma ->M10 * mb ->M01 + ma ->M11 * mb ->M11 + ma ->M12 * mb ->M21 + ma ->M13 * mb ->M31 ;  
653.     mprod ->M12 = ma ->M10 * mb ->M02 + ma ->M11 * mb ->M12 + ma ->M12 * mb ->M22 + ma ->M13 * mb ->M32 ;  
654.     mprod ->M13 = ma ->M10 * mb ->M03 + ma ->M11 * mb ->M13 + ma ->M12 * mb ->M23 + ma ->M13 * mb ->M33 ;  
655.     mprod ->M20 = ma ->M20 * mb ->M00 + ma ->M21 * mb ->M10 + ma ->M22 * mb ->M20 + ma ->M23 * mb ->M30 ;  
656.     mprod ->M21 = ma ->M20 * mb ->M01 + ma ->M21 * mb ->M11 + ma ->M22 * mb ->M21 + ma ->M23 * mb ->M31 ;  
657.     mprod ->M22 = ma ->M20 * mb ->M02 + ma ->M21 * mb ->M12 + ma ->M22 * mb ->M22 + ma ->M23 * mb ->M32 ;  
658.     mprod ->M23 = ma ->M20 * mb ->M03 + ma ->M21 * mb ->M13 + ma ->M22 * mb ->M23 + ma ->M23 * mb ->M33 ;  
659.     mprod ->M30 = ma ->M30 * mb ->M00 + ma ->M31 * mb ->M10 + ma ->M32 * mb ->M20 + ma ->M33 * mb ->M30 ;  
660.     mprod ->M31 = ma ->M30 * mb ->M01 + ma ->M31 * mb ->M11 + ma ->M32 * mb ->M21 + ma ->M33 * mb ->M31 ;  
661.     mprod ->M32 = ma ->M30 * mb ->M02 + ma ->M31 * mb ->M12 + ma ->M32 * mb ->M22 + ma ->M33 * mb ->M32 ;  
662.     mprod ->M33 = ma ->M30 * mb ->M03 + ma ->M31 * mb ->M13 + ma ->M32 * mb ->M23 + ma ->M33 * mb ->M33 ;  
663. }  
664.   
665. bool Mat_Inverse_4X4(MATRIX4X4_PTR m, MATRIX4X4_PTR mi)  
666. {  
667.     float det = Mat_Det_3X3(reinterpret_cast<MATRIX3X3_PTR>(m)) ;  
668.   
669.     if (fabs(det) < EPSILON_E5)  
670.         return false ;  
671.   
672.     mi ->M00 = (m ->M11 * m ->M22 - m ->M12 * m ->M21) / det ;  
673.     mi ->M01 = -(m ->M01 * m ->M22 - m ->M02 * m ->M21) / det ;  
674.     mi ->M02 = (m ->M01 * m ->M12 - m ->M02 * m ->M11 ) / det ;  
675.     mi ->M03 = 0 ;  
676.   
677.     mi ->M10 = -( m ->M10 * m ->M22 - m ->M12 * m ->M20 ) / det ;  
678.     mi ->M11 =  ( m ->M00 * m ->M22 - m ->M02 * m ->M20 ) / det ;  
679.     mi ->M12 = -( m ->M00 * m ->M12 - m ->M02 * m ->M10 ) / det ;  
680.     mi ->M13 = 0 ;  
681.   
682.     mi ->M20 = ( m ->M10 * m ->M21 - m ->M11 * m ->M20 ) / det ;  
683.     mi ->M21 = -( m ->M00 * m ->M21 - m ->M01 * m ->M20 ) / det ;  
684.     mi ->M22 = ( m ->M00 * m ->M11 - m ->M01 * m ->M10 ) / det ;  
685.     mi ->M23 = 0 ;  
686.   
687.     mi ->M30 = -( m ->M30 * mi ->M00 + m ->M31 * mi ->M10 + m ->M32 * mi ->M20 ) / det ;  
688.     mi ->M31 = -( m ->M30 * mi ->M01 + m ->M31 * mi ->M11 + m ->M32 * mi ->M21 ) / det ;  
689.     mi ->M32 = -( m ->M30 * mi ->M02 + m ->M31 * mi ->M12 + m ->M32 * mi ->M22 ) / det ;  
690.     mi ->M33 = 1.0f;  
691.   
692.     return true ;  
693. }  
694.   
695. void Mat_Mul_VECTOR3D_4X4(VECTOR3D_PTR va, MATRIX4X4_PTR mb, VECTOR3D_PTR vprod)  
696. {  
697.     for (int col = 0; col < 3; ++col)  
698.     {  
699.         float sum = 0 ;  
700.   
701.         for (int row = 0; row < 3; ++row)  
702.         {  
703.             sum += va ->M[row] * mb ->M[row][col] ;  
704.         }  
705.   
706.         sum += mb ->M[3][col] ;  
707.   
708.         vprod ->M[col] = sum ;  
709.     }  
710. }  
711.   
712. void Mat_Mul_VECTOR3D_4X3(VECTOR3D_PTR va, MATRIX4X3_PTR mb, VECTOR3D_PTR vprod)  
713. {  
714.     for (int col = 0; col < 3; ++col)  
715.     {  
716.         float sum = 0 ;  
717.   
718.         for (int row = 0; row < 3; ++row)  
719.         {  
720.             sum += va ->M[row] * mb ->M[row][col] ;  
721.         }  
722.   
723.         sum += mb ->M[3][col] ;  
724.   
725.         vprod ->M[col] = sum ;  
726.     }  
727. }  
728.   
729. void Mat_Mul_VECTOR4D_4X4(VECTOR4D_PTR va, MATRIX4X4_PTR mb, VECTOR4D_PTR vprod)  
730. {  
731.     for (int col = 0; col < 4; ++col)  
732.     {  
733.         float sum = 0 ;  
734.   
735.         for (int row = 0; row < 4; ++row)  
736.         {  
737.             sum += va ->M[col] * mb ->M[row][col] ;  
738.         }  
739.   
740.         vprod ->M[col] = sum ;  
741.     }  
742. }  
743.   
744. void Mat_Mul_VECTOR4D_4X3(VECTOR4D_PTR va, MATRIX4X3_PTR mb, VECTOR4D_PTR vprod)  
745. {  
746.     for (int col = 0; col < 3; ++col)  
747.     {  
748.         float sum = 0 ;  
749.   
750.         for (int row = 0; row < 4; ++row)  
751.         {  
752.             sum += va ->M[col] * mb ->M[col][row] ;  
753.         }  
754.   
755.         vprod ->M[col] = sum ;  
756.     }  
757. }  
758.   
759. void Mat_Mul_1X4_4X4(MATRIX1X4_PTR ma, MATRIX4X4_PTR mb, MATRIX1X4_PTR mprod)  
760. {  
761.     mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + ma ->M02 * mb ->M20 + ma ->M03 * mb ->M30 ;  
762.     mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + ma ->M02 * mb ->M21 + ma ->M03 * mb ->M31 ;  
763.     mprod ->M02 = ma ->M00 * mb ->M02 + ma ->M01 * mb ->M12 + ma ->M02 * mb ->M22 + ma ->M03 * mb ->M32 ;  
764.     mprod ->M03 = ma ->M00 * mb ->M03 + ma ->M01 * mb ->M13 + ma ->M02 * mb ->M23 + ma ->M03 * mb ->M33 ;  
765. }  
766.   
767. void Init_Parm_Line2D(POINT2D_PTR p_init, POINT2D_PTR p_term, PARMLINE2D_PTR p)  
768. {  
769.     VECTOR2D_INIT(&(p ->p0), p_init) ;  
770.     VECTOR2D_INIT(&(p ->p1), p_term) ;  
771.     VECTOR2D_Build(p_init, p_term, &(p ->v)) ;  
772. }  
773.   
774. void Compute_Parm_Line2D(PARMLINE2D_PTR p, float t, POINT2D_PTR pt)  
775. {  
776.     pt ->x = p ->p0.x + p ->v.x * t ;  
777.     pt ->y = p ->p0.y + p ->v.y * t ;  
778. }  
779.   
780. int Intersect_Parm_Line2D(PARMLINE2D_PTR p1, PARMLINE2D_PTR p2, float * t1, float * t2)  
781. {  
782.     //  y1/x1 - y2/x2 = y1 * x2 - y2 * x1  
783.     float det_p1p2 = (p1 ->v.x * p2 ->v.y - p1 ->v.y * p2 ->v.x) ;  
784.   
785.     if (fabs(det_p1p2) <= EPSILON_E5)  
786.         return PARM_LINE_NO_INTERSECT ;  
787.   
788.     *t1 = (p1 ->v.x * (p1 ->p0.y - p2 ->p0.y) - p2 ->v.y * (p1 ->p0.x - p2 ->p0.x)) / det_p1p2 ;  
789.     *t2 = (p1 ->v.x * (p1 ->p0.y - p2 ->p0.y) - p1 ->v.y * (p1 ->p0.x - p2 ->p0.x)) / det_p1p2 ;  
790.   
791.   
792.     if (*t1 >= 0 && *t1 <= 1 &&  
793.         *t2 >= 0 && *t2 <= 1)  
794.     {  
795.         return PARM_LINE_INTERSECT_IN_SEGMENT ;  
796.     }  
797.     else  
798.     {  
799.         return PARM_LINE_INTERSECT_OUT_SEGMENT ;  
800.     }  
801. }  
802.   
803. int Intersect_Parm_Lines2D(PARMLINE2D_PTR p1, PARMLINE2D_PTR p2, POINT2D_PTR pt)  
804. {  
805.     float t1 ;  
806.     float t2 ;  
807.     float det_p1p2 = (p1 ->v.x * p2 ->v.y - p1 ->v.y * p2 ->v.x) ;  
808.   
809.     if (fabs(det_p1p2) <= EPSILON_E5)  
810.         return PARM_LINE_NO_INTERSECT ;  
811.   
812.     t1 = (p2 ->v.x * (p1 ->p0.y - p2 ->p0.y) - p2 ->v.y * (p1 ->p0.x - p2 ->p0.x))  
813.             / det_p1p2;  
814.   
815.     t2 = (p1 ->v.x * (p1 ->p0.y - p2 ->p0.y) - p1->v.y * (p1 ->p0.x - p2 ->p0.x))  
816.             / det_p1p2;  
817.   
818.     pt ->x = p1 ->p0.x + p1 ->v.x * t1 ;  
819.     pt ->y = p1 ->p0.y + p1 ->v.y * t1 ;  
820.   
821.     if (t1 >= 0 && t1 <= 1 &&  
822.         t2 >= 0 && t2 <= 1)  
823.     {  
824.         return PARM_LINE_INTERSECT_IN_SEGMENT ;  
825.     }  
826.     else  
827.     {  
828.         return PARM_LINE_INTERSECT_OUT_SEGMENT ;  
829.     }  
830. }  
831.   
832. void Init_Parm_Line3D(POINT3D_PTR p_init, POINT3D_PTR p_term, PARMLINE3D_PTR p)  
833. {  
834.     VECTOR3D_INIT(&(p ->p0), p_init) ;  
835.   
836.     VECTOR3D_INIT(&(p ->p1), p_term) ;  
837.   
838.     VECTOR3D_Build(p_init, p_term, &(p ->V)) ;  
839. }  
840.   
841. void Compute_Parm_Line3D(PARMLINE3D_PTR p, float t, POINT3D_PTR pt)  
842. {  
843.     pt ->x = p ->p0.x + p ->V.x * t ;  
844.     pt ->y = p ->p0.y + p ->V.y * t ;  
845.     pt ->z = p ->p0.z + p ->V.z * t ;  
846. }  
847.   
848. void POINT3D_COPY(POINT3D_PTR vdst, POINT3D_PTR vsrc)  
849. {  
850.     vdst ->x = vsrc ->x ;  
851.     vdst ->y = vsrc ->y ;  
852.     vdst ->z = vsrc ->z ;  
853. }  
854.   
855. void VECTOR3D_COPY(VECTOR3D_PTR vdst, VECTOR3D_PTR vsrc)  
856. {  
857.     vdst ->x = vsrc ->x ;  
858.     vdst ->y = vsrc ->y ;  
859.     vdst ->z = vsrc ->z ;  
860. }  
861.   
862. void PLANE3D_Init(PLANE3D_PTR plane, POINT3D_PTR p0, VECTOR3D_PTR normal, bool normalize)  
863. {  
864.     POINT3D_COPY(&(plane ->p0), p0) ;  
865.   
866.     if (!normalize)  
867.         VECTOR3D_COPY(&(plane ->n), normal) ;  
868.     else  
869.         VECTOR3D_Normalize(normal, &(plane ->n)) ;  
870. }  
871.   
872. float Compute_Point_In_Plane3D(POINT3D_PTR pt, PLANE3D_PTR plane)  
873. {  
874.     float hs = plane ->n.x * (pt ->x - plane ->p0.x) +  
875.         plane ->n.y * (pt ->y - plane ->p0.y) +  
876.         plane ->n.z * (pt ->z - plane ->p0.z) ;  
877.   
878.     return hs ;  
879. }  
880.   
881. int Intersect_Parm_Line3D_Plane3D(PARMLINE3D_PTR pline, PLANE3D_PTR plane, float *t, POINT3D_PTR pt)  
882. {  
883.     float plane_dot_line = VECTOR3D_Dot(&(pline ->V), &(plane ->n)) ;  
884.        
885.     if (fabs(plane_dot_line) <= EPSILON_E5)  
886.     {  
887.         if (fabs(Compute_Point_In_Plane3D(&(pline ->p0), plane)) <= EPSILON_E5)  
888.             return PARM_LINE_INTERSECT_EVERYWHERE ;  
889.         else  
890.             return PARM_LINE_NO_INTERSECT ;  
891.     }  
892.   
893.     *t = -(plane ->n.x * pline ->p0.x + plane ->n.y * pline ->p0.y + plane ->n.z * pline ->p0.z +  
894.         (-plane ->n.x * plane ->p0.x - plane ->n.y * plane ->p0.y - plane ->n.z * plane ->p0.z)) /  
895.         plane_dot_line ;  
896.   
897.     pt ->x = pline ->p0.x + pline ->V.x * (*t) ;  
898.     pt ->y = pline ->p0.y + pline ->V.y * (*t) ;  
899.     pt ->z = pline ->p0.z + pline ->V.z * (*t) ;  
900.   
901.     if (*t >= 0 && *t <= 1)  
902.         return PARM_LINE_INTERSECT_IN_SEGMENT ;  
903.     else  
904.         return PARM_LINE_INTERSECT_OUT_SEGMENT ;  
905. }  
906.   
907. void VECTOR3D_Theta_To_QUAT(QUAT_PTR q, VECTOR3D_PTR v, float theta)  
908. {  
909.     float theta_div_2 = theta / 2 ;  
910.   
911.     float sinf_theta = sinf(theta_div_2) ;  
912.   
913.     q ->x = sinf_theta * v ->x ;  
914.     q ->y = sinf_theta * v ->y ;  
915.     q ->z = sinf_theta * v ->z ;  
916.     q ->w = cosf(theta_div_2) ;  
917. }  
918.   
919. void VECTOR4D_Theta_To_QUAT(QUAT_PTR q, VECTOR4D_PTR v, float theta)  
920. {  
921.     float theta_div_2 = theta / 2 ;  
922.   
923.     float sinf_theta = sinf(theta_div_2) ;  
924.   
925.     q ->x = sinf_theta * v ->x ;  
926.     q ->y = sinf_theta * v ->y ;  
927.     q ->z = sinf_theta * v ->z ;  
928.     q ->w = cosf(theta_div_2) ;  
929. }  
930.   
931. void EulerZYX_To_QUAT(QUAT_PTR q, float theta_z, float theta_y, float theta_x)  
932. {  
933.     float cos_z_2 = cosf(theta_z) / 2 ;  
934.     float cos_y_2 = cosf(theta_y) / 2 ;  
935.     float cos_x_2 = cosf(theta_x) / 2 ;  
936.   
937.     float sin_z_2 = sinf(theta_z) / 2 ;  
938.     float sin_y_2 = sinf(theta_y) / 2 ;  
939.     float sin_x_2 = sinf(theta_x) / 2 ;  
940.   
941.     q ->w = cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2 ;  
942.     q ->x = cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2 ;  
943.     q ->y = cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2 ;  
944.     q ->z = cos_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_2 * sin_x_2 ;  
945. }  
946.   
947. void QUAT_To_VECTOR3D_Theta(QUAT_PTR q, VECTOR3D_PTR v, float * theta)  
948. {  
949.     *theta = acosf(q ->w) ;  
950.   
951.     float sin_theta = sinf(*theta) ;  
952.   
953.     v ->x = q ->x / sin_theta ;  
954.     v ->y = q ->y / sin_theta ;  
955.     v ->z = q ->z / sin_theta ;  
956.   
957.     *theta *= 2 ;  
958. }  
959.   
960. void QUAT_Add(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qsum)  
961. {  
962.     qsum ->x = q1 ->x + q2 ->x ;  
963.     qsum ->y = q1 ->y + q2 ->y ;  
964.     qsum ->z = q1 ->z + q2 ->z ;  
965.     qsum ->w = q1 ->w + q2 ->w ;  
966. }  
967.   
968. void QUAT_Sub(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qdiff)  
969. {  
970.     qdiff ->x = q1 ->x - q2 ->x ;  
971.     qdiff ->y = q1 ->y - q2 ->y ;  
972.     qdiff ->z = q1 ->z - q2 ->z ;  
973.     qdiff ->w = q1 ->w - q2 ->w ;  
974. }  
975.   
976. void QUAT_Conjugate(QUAT_PTR q, QUAT_PTR qconj)  
977. {  
978.     qconj ->x = -q ->x ;  
979.     qconj ->y = -q ->y ;  
980.     qconj ->z = -q ->z ;  
981.     qconj ->w = q ->w ;  
982. }  
983.   
984. void QUAT_Scale(QUAT_PTR q, float scale, QUAT_PTR qs)  
985. {  
986.     qs ->x = scale * q ->x ;  
987.     qs ->y = scale * q ->y ;  
988.     qs ->z = scale * q ->z ;  
989.     qs ->w = scale * q ->w ;  
990. }  
991.   
992. void QUAT_Scale(QUAT_PTR q, float scale)  
993. {  
994.     q ->x *= scale ;  
995.     q ->y *= scale ;  
996.     q ->z *= scale ;  
997.     q ->w *= scale ;  
998. }  
999.   
1000. float QUAT_Norm(QUAT_PTR q)  
1001. {  
1002.     return sqrtf(q ->w * q ->w + q ->x * q ->x + q ->y * q ->y + q ->z * q ->z) ;  
1003. }  
1004.   
1005. float QUAT_Norm2(QUAT_PTR q)  
1006. {  
1007.     return q ->w * q ->w + q ->x * q ->x + q ->y * q ->y + q ->z * q ->z ;  
1008. }  
1009.   
1010. void QUAT_Normalize(QUAT_PTR q, QUAT_PTR qn)  
1011. {  
1012.     float length = QUAT_Norm(q) ;  
1013.   
1014.     qn ->w = q ->w / length ;  
1015.     qn ->x = q ->x / length ;  
1016.     qn ->y = q ->y / length ;  
1017.     qn ->z = q ->z / length ;  
1018. }  
1019.   
1020. void QUAT_Normalize(QUAT_PTR q)  
1021. {  
1022.     float length = QUAT_Norm(q) ;  
1023.   
1024.     q ->w /= length ;  
1025.     q ->x /= length ;  
1026.     q ->y /= length ;  
1027.     q ->z /= length ;  
1028. }  
1029.   
1030. void QUAT_Unit_Inverse(QUAT_PTR q, QUAT_PTR qi)  
1031. {  
1032.     qi ->w = q ->w ;  
1033.     qi ->x = -q ->x ;  
1034.     qi ->y = -q ->y ;  
1035.     qi ->z = -q ->z ;  
1036. }  
1037.   
1038. void QUAT_Unit_Inverse(QUAT_PTR q)  
1039. {  
1040.     q ->x = -q ->x ;  
1041.     q ->y = -q ->y ;  
1042.     q ->z = -q ->z ;  
1043. }  
1044.   
1045. void QUAT_Mul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod)  
1046. {  
1047.     qprod ->w = q1 ->w * q2 ->w - q1 ->x * q2 ->x - q1 ->y * q2 ->y - q1 ->z * q2 ->z ;  
1048.     qprod ->x = q1 ->w * q2 ->w + q2 ->x * q2 ->w + q1 ->y * q2 ->z - q1 ->z * q2 ->y ;  
1049.     qprod ->y = q1 ->w * q2 ->y - q1 ->x * q2 ->z + q1 ->y * q2 ->w - q1 ->z * q2 ->x ;  
1050.     qprod ->z = q1 ->w * q2 ->z + q1 ->x * q2 ->y - q1 ->y * q2 ->x + q1 ->z * q2 ->w ;  
1051. }  
1052.   
1053. void QUAT_Triple_Product(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, QUAT_PTR qprod)  
1054. {  
1055.     QUAT qtemp ;  
1056.     QUAT_Mul(q1, q2, &qtemp) ;  
1057.     QUAT_Mul(&qtemp, q3, qprod) ;  
1058. }  
1059.   
1060. bool Solve_2X2_System(MATRIX2X2_PTR A, MATRIX1X2_PTR X, MATRIX1X2_PTR B)  
1061. {  
1062.     float det_A = Mat_Det_2X2(A) ;  
1063.   
1064.     if (fabs(det_A) < EPSILON_E5)  
1065.         return false ;  
1066.   
1067.     MATRIX2X2 work_mat ;  
1068.   
1069.     MAT_COPY_2X2(A, &work_mat) ;  
1070.   
1071.     MAT_COLUMN_SWAP_2X2(&work_mat, 0, B) ;  
1072.   
1073.     float det_ABx = Mat_Det_2X2(&work_mat) ;  
1074.   
1075.     X ->M00 = det_ABx / det_A ;  
1076.   
1077.     MAT_COPY_2X2(A, &work_mat) ;  
1078.   
1079.     MAT_COLUMN_SWAP_2X2(&work_mat, 1, B) ;  
1080.   
1081.     float det_ABy = Mat_Det_2X2(&work_mat) ;  
1082.   
1083.     X ->M01 = det_ABy / det_A ;  
1084.   
1085.     return true ;  
1086. }  
1087.   
1088. bool Solve_3X3_System(MATRIX3X3_PTR A, MATRIX1X3_PTR X, MATRIX1X3_PTR B)  
1089. {  
1090.     float det_A = Mat_Det_3X3(A);  
1091.   
1092.     if (fabs(det_A) < EPSILON_E5)  
1093.         return false ;  
1094.   
1095.     MATRIX3X3 work_mat;   
1096.   
1097.     MAT_COPY_3X3(A, &work_mat);  
1098.   
1099.     MAT_COLUMN_SWAP_3X3(&work_mat, 0, B);  
1100.   
1101.     float det_ABx = Mat_Det_3X3(&work_mat);  
1102.   
1103.     X->M00 = det_ABx/det_A;  
1104.   
1105.     MAT_COPY_3X3(A, &work_mat);  
1106.   
1107.     MAT_COLUMN_SWAP_3X3(&work_mat, 1, B);  
1108.   
1109.     float det_ABy = Mat_Det_3X3(&work_mat);  
1110.   
1111.     X->M01 = det_ABy/det_A;  
1112.   
1113.     MAT_COPY_3X3(A, &work_mat);  
1114.   
1115.     MAT_COLUMN_SWAP_3X3(&work_mat, 2, B);  
1116.   
1117.     float det_ABz = Mat_Det_3X3(&work_mat);  
1118.   
1119.     X->M02 = det_ABz/det_A;  
1120.   
1121.     return true ;  
1122. }  

非常感谢@Golden_Shadow 对我这样的半吊子有了例程引擎的数学部分就简单多了，但是

#include "MyMath.h"

这个库是什么鬼？！