diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs | 581 |
1 files changed, 581 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs new file mode 100644 index 0000000..2e3f258 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs | |||
@@ -0,0 +1,581 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | using MonoXnaCompactMaths; | ||
27 | |||
28 | namespace XnaDevRu.BulletX | ||
29 | { | ||
30 | public static class MathHelper | ||
31 | { | ||
32 | internal const float Sqrt12 = 0.7071067811865475244008443621048490f; | ||
33 | internal const float Infinity = 3.402823466e+38f; | ||
34 | internal const float Epsilon = 1.192092896e-07f; | ||
35 | |||
36 | public static Vector3 MatrixToVector(Matrix m, Vector3 v) | ||
37 | { | ||
38 | return new Vector3( | ||
39 | Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X, | ||
40 | Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y, | ||
41 | Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z | ||
42 | ); | ||
43 | } | ||
44 | |||
45 | internal static int ClosestAxis(Vector4 v) | ||
46 | { | ||
47 | return MaxAxis(Absolute(v)); | ||
48 | } | ||
49 | |||
50 | internal static Vector4 Absolute(Vector4 v) | ||
51 | { | ||
52 | return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W)); | ||
53 | } | ||
54 | |||
55 | internal static int MaxAxis(Vector4 v) | ||
56 | { | ||
57 | int maxIndex = -1; | ||
58 | float maxVal = float.MinValue; | ||
59 | if (v.X > maxVal) | ||
60 | { | ||
61 | maxIndex = 0; | ||
62 | maxVal = v.X; | ||
63 | } | ||
64 | if (v.Y > maxVal) | ||
65 | { | ||
66 | maxIndex = 1; | ||
67 | maxVal = v.Y; | ||
68 | } | ||
69 | if (v.Z > maxVal) | ||
70 | { | ||
71 | maxIndex = 2; | ||
72 | maxVal = v.Z; | ||
73 | } | ||
74 | if (v.W > maxVal) | ||
75 | { | ||
76 | maxIndex = 3; | ||
77 | maxVal = v.W; | ||
78 | } | ||
79 | |||
80 | return maxIndex; | ||
81 | } | ||
82 | |||
83 | internal static int MaxAxis(Vector3 v) | ||
84 | { | ||
85 | return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0); | ||
86 | } | ||
87 | |||
88 | // conservative test for overlap between two aabbs | ||
89 | internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB) | ||
90 | { | ||
91 | bool overlap = true; | ||
92 | overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap; | ||
93 | overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap; | ||
94 | overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap; | ||
95 | return overlap; | ||
96 | } | ||
97 | |||
98 | internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax) | ||
99 | { | ||
100 | Vector3 p1 = vertices[0]; | ||
101 | Vector3 p2 = vertices[1]; | ||
102 | Vector3 p3 = vertices[2]; | ||
103 | |||
104 | if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false; | ||
105 | if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false; | ||
106 | |||
107 | if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false; | ||
108 | if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false; | ||
109 | |||
110 | if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false; | ||
111 | if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false; | ||
112 | return true; | ||
113 | } | ||
114 | |||
115 | internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated) | ||
116 | { | ||
117 | float s = 1.0f - rt; | ||
118 | interpolated.X = s * vA.X + rt * vB.X; | ||
119 | interpolated.Y = s * vA.Y + rt * vB.Y; | ||
120 | interpolated.Z = s * vA.Z + rt * vB.Z; | ||
121 | } | ||
122 | |||
123 | internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q) | ||
124 | { | ||
125 | if (Math.Abs(n.Z) > Sqrt12) | ||
126 | { | ||
127 | // choose p in y-z plane | ||
128 | float a = n.Y * n.Y + n.Z * n.Z; | ||
129 | float k = 1f / (float)Math.Sqrt(a); | ||
130 | p.X = 0; | ||
131 | p.Y = -n.Z * k; | ||
132 | p.Z = n.Y * k; | ||
133 | // set q = n x p | ||
134 | q.X = a * k; | ||
135 | q.Y = -n.X * p.Z; | ||
136 | q.Z = n.X * p.Y; | ||
137 | } | ||
138 | else | ||
139 | { | ||
140 | // choose p in x-y plane | ||
141 | float a = n.X * n.X + n.Y * n.Y; | ||
142 | float k = 1f / (float)Math.Sqrt(a); | ||
143 | p.X = -n.Y * k; | ||
144 | p.Y = n.X * k; | ||
145 | p.Z = 0; | ||
146 | // set q = n x p | ||
147 | q.X = -n.Z * p.Y; | ||
148 | q.Y = n.Z * p.X; | ||
149 | q.Z = a * k; | ||
150 | } | ||
151 | } | ||
152 | |||
153 | internal static bool RayAabb(Vector3 rayFrom, | ||
154 | Vector3 rayTo, | ||
155 | Vector3 aabbMin, | ||
156 | Vector3 aabbMax, | ||
157 | float param, Vector3 normal) | ||
158 | { | ||
159 | Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f; | ||
160 | Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f; | ||
161 | Vector3 source = rayFrom - aabbCenter; | ||
162 | Vector3 target = rayTo - aabbCenter; | ||
163 | int sourceOutcode = Outcode(source, aabbHalfExtent); | ||
164 | int targetOutcode = Outcode(target, aabbHalfExtent); | ||
165 | if ((sourceOutcode & targetOutcode) == 0x0) | ||
166 | { | ||
167 | float lambda_enter = 0; | ||
168 | float lambda_exit = param; | ||
169 | Vector3 r = target - source; | ||
170 | float normSign = 1; | ||
171 | Vector3 hitNormal = new Vector3(); | ||
172 | int bit = 1; | ||
173 | |||
174 | for (int j = 0; j < 2; j++) | ||
175 | { | ||
176 | { | ||
177 | if ((sourceOutcode & bit) != 0) | ||
178 | { | ||
179 | float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; | ||
180 | if (lambda_enter <= lambda) | ||
181 | { | ||
182 | lambda_enter = lambda; | ||
183 | hitNormal = new Vector3(); | ||
184 | hitNormal.X = normSign; | ||
185 | } | ||
186 | } | ||
187 | else if ((targetOutcode & bit) != 0) | ||
188 | { | ||
189 | float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; | ||
190 | SetMin(ref lambda_exit, lambda); | ||
191 | } | ||
192 | bit <<= 1; | ||
193 | } | ||
194 | { | ||
195 | if ((sourceOutcode & bit) != 0) | ||
196 | { | ||
197 | float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; | ||
198 | if (lambda_enter <= lambda) | ||
199 | { | ||
200 | lambda_enter = lambda; | ||
201 | hitNormal = new Vector3(); | ||
202 | hitNormal.Y = normSign; | ||
203 | } | ||
204 | } | ||
205 | else if ((targetOutcode & bit) != 0) | ||
206 | { | ||
207 | float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; | ||
208 | SetMin(ref lambda_exit, lambda); | ||
209 | } | ||
210 | bit <<= 1; | ||
211 | } | ||
212 | { | ||
213 | if ((sourceOutcode & bit) != 0) | ||
214 | { | ||
215 | float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; | ||
216 | if (lambda_enter <= lambda) | ||
217 | { | ||
218 | lambda_enter = lambda; | ||
219 | hitNormal = new Vector3(); | ||
220 | hitNormal.Z = normSign; | ||
221 | } | ||
222 | } | ||
223 | else if ((targetOutcode & bit) != 0) | ||
224 | { | ||
225 | float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; | ||
226 | SetMin(ref lambda_exit, lambda); | ||
227 | } | ||
228 | bit <<= 1; | ||
229 | } | ||
230 | normSign = -1; | ||
231 | } | ||
232 | if (lambda_enter <= lambda_exit) | ||
233 | { | ||
234 | param = lambda_enter; | ||
235 | normal = hitNormal; | ||
236 | return true; | ||
237 | } | ||
238 | } | ||
239 | return false; | ||
240 | } | ||
241 | |||
242 | internal static void SetMin(ref float a, float b) | ||
243 | { | ||
244 | if (a > b) | ||
245 | a = b; | ||
246 | } | ||
247 | |||
248 | internal static void SetMax(ref float a, float b) | ||
249 | { | ||
250 | if (a < b) | ||
251 | a = b; | ||
252 | } | ||
253 | |||
254 | internal static void SetMax(ref Vector3 self, Vector3 other) | ||
255 | { | ||
256 | if (other.X > self.X) | ||
257 | self.X = other.X; | ||
258 | |||
259 | if (other.Y > self.Y) | ||
260 | self.Y = other.Y; | ||
261 | |||
262 | if (other.Z > self.Z) | ||
263 | self.Z = other.Z; | ||
264 | } | ||
265 | |||
266 | internal static Vector3 SetMax(Vector3 self, Vector3 other) | ||
267 | { | ||
268 | if (other.X > self.X) | ||
269 | self.X = other.X; | ||
270 | |||
271 | if (other.Y > self.Y) | ||
272 | self.Y = other.Y; | ||
273 | |||
274 | if (other.Z > self.Z) | ||
275 | self.Z = other.Z; | ||
276 | |||
277 | return self; | ||
278 | } | ||
279 | |||
280 | internal static void SetMin(ref Vector3 self, Vector3 other) | ||
281 | { | ||
282 | if (other.X < self.X) | ||
283 | self.X = other.X; | ||
284 | |||
285 | if (other.Y < self.Y) | ||
286 | self.Y = other.Y; | ||
287 | |||
288 | if (other.Z < self.Z) | ||
289 | self.Z = other.Z; | ||
290 | } | ||
291 | |||
292 | internal static Vector3 SetMin(Vector3 self, Vector3 other) | ||
293 | { | ||
294 | if (other.X < self.X) | ||
295 | self.X = other.X; | ||
296 | |||
297 | if (other.Y < self.Y) | ||
298 | self.Y = other.Y; | ||
299 | |||
300 | if (other.Z < self.Z) | ||
301 | self.Z = other.Z; | ||
302 | |||
303 | return self; | ||
304 | } | ||
305 | |||
306 | internal static int Outcode(Vector3 p, Vector3 halfExtent) | ||
307 | { | ||
308 | return (p.X < -halfExtent.X ? 0x01 : 0x0) | | ||
309 | (p.X > halfExtent.X ? 0x08 : 0x0) | | ||
310 | (p.Y < -halfExtent.Y ? 0x02 : 0x0) | | ||
311 | (p.Y > halfExtent.Y ? 0x10 : 0x0) | | ||
312 | (p.Z < -halfExtent.Z ? 0x4 : 0x0) | | ||
313 | (p.Z > halfExtent.Z ? 0x20 : 0x0); | ||
314 | } | ||
315 | |||
316 | internal static Matrix Absolute(Matrix m) | ||
317 | { | ||
318 | return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14), | ||
319 | Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24), | ||
320 | Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34), | ||
321 | Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44)); | ||
322 | } | ||
323 | |||
324 | internal static void SetValueByIndex(ref Vector3 v, int i, float value) | ||
325 | { | ||
326 | if (i == 0) | ||
327 | v.X = value; | ||
328 | else if (i == 1) | ||
329 | v.Y = value; | ||
330 | else | ||
331 | v.Z = value; | ||
332 | } | ||
333 | |||
334 | internal static float GetValueByIndex(Vector3 v, int i) | ||
335 | { | ||
336 | if (i == 0) | ||
337 | return v.X; | ||
338 | else if (i == 1) | ||
339 | return v.Y; | ||
340 | else | ||
341 | return v.Z; | ||
342 | } | ||
343 | |||
344 | internal static Vector3 InvXForm(Matrix m, Vector3 v) | ||
345 | { | ||
346 | v -= m.Translation; | ||
347 | m.Translation = new Vector3(); | ||
348 | return MathHelper.Transform(v, Matrix.Transpose(m)); | ||
349 | } | ||
350 | |||
351 | internal static Matrix InverseTimes(Matrix m, Matrix t) | ||
352 | { | ||
353 | Vector3 v = t.Translation - m.Translation; | ||
354 | |||
355 | Matrix mat = TransposeTimes(m, t); | ||
356 | mat.Translation = Vector3.Transform(v, m); | ||
357 | return mat; | ||
358 | } | ||
359 | |||
360 | internal static Matrix TransposeTimes(Matrix mA, Matrix mB) | ||
361 | { | ||
362 | return new Matrix( | ||
363 | mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31, | ||
364 | mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32, | ||
365 | mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33, | ||
366 | 0, | ||
367 | mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31, | ||
368 | mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32, | ||
369 | mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33, | ||
370 | 0, | ||
371 | mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31, | ||
372 | mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32, | ||
373 | mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33, | ||
374 | 0, 0, 0, 0, 1); | ||
375 | } | ||
376 | |||
377 | internal static Vector3 GetColumn(Matrix m, int column) | ||
378 | { | ||
379 | switch (column) | ||
380 | { | ||
381 | case 1: | ||
382 | return new Vector3(m.M11, m.M21, m.M31); | ||
383 | case 2: | ||
384 | return new Vector3(m.M12, m.M22, m.M32); | ||
385 | case 3: | ||
386 | return new Vector3(m.M13, m.M23, m.M33); | ||
387 | default: | ||
388 | throw new ArgumentOutOfRangeException("column"); | ||
389 | } | ||
390 | } | ||
391 | |||
392 | internal static Vector3 GetRow(Matrix m, int row) | ||
393 | { | ||
394 | switch (row) | ||
395 | { | ||
396 | case 1: | ||
397 | return new Vector3(m.M11, m.M12, m.M13); | ||
398 | case 2: | ||
399 | return new Vector3(m.M21, m.M22, m.M23); | ||
400 | case 3: | ||
401 | return new Vector3(m.M31, m.M32, m.M33); | ||
402 | default: | ||
403 | throw new ArgumentOutOfRangeException("row"); | ||
404 | } | ||
405 | } | ||
406 | |||
407 | internal static Quaternion GetRotation(Matrix m) | ||
408 | { | ||
409 | float trace = m.M11 + m.M22 + m.M33; | ||
410 | Quaternion q = new Quaternion(); | ||
411 | |||
412 | if (trace > 0) | ||
413 | { | ||
414 | float s = (float)Math.Sqrt(trace + 1.0f); | ||
415 | q.W = s * 0.5f; | ||
416 | s = 0.5f / s; | ||
417 | |||
418 | q.X = (m.M32 - m.M23) * s; | ||
419 | q.Y = (m.M13 - m.M31) * s; | ||
420 | q.Z = (m.M21 - m.M12) * s; | ||
421 | } | ||
422 | else | ||
423 | { | ||
424 | int i = m.M11 < m.M22 ? | ||
425 | (m.M22 < m.M33 ? 2 : 1) : | ||
426 | (m.M11 < m.M33 ? 2 : 0); | ||
427 | int j = (i + 1) % 3; | ||
428 | int k = (i + 2) % 3; | ||
429 | |||
430 | float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f); | ||
431 | SetElement(ref q, i, s * 0.5f); | ||
432 | s = 0.5f / s; | ||
433 | |||
434 | q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s; | ||
435 | SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s); | ||
436 | SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s); | ||
437 | } | ||
438 | return q; | ||
439 | } | ||
440 | |||
441 | internal static float SetElement(ref Quaternion q, int index, float value) | ||
442 | { | ||
443 | switch (index) | ||
444 | { | ||
445 | case 0: | ||
446 | q.X = value; break; | ||
447 | case 1: | ||
448 | q.Y = value; break; | ||
449 | case 2: | ||
450 | q.Z = value; break; | ||
451 | case 3: | ||
452 | q.W = value; break; | ||
453 | } | ||
454 | |||
455 | return 0; | ||
456 | } | ||
457 | |||
458 | internal static float GetElement(Quaternion q, int index) | ||
459 | { | ||
460 | switch (index) | ||
461 | { | ||
462 | case 0: | ||
463 | return q.X; | ||
464 | case 1: | ||
465 | return q.Y; | ||
466 | case 2: | ||
467 | return q.Z; | ||
468 | default: | ||
469 | return q.W; | ||
470 | } | ||
471 | } | ||
472 | |||
473 | internal static float GetElement(Matrix mat, int index) | ||
474 | { | ||
475 | int row = index % 3; | ||
476 | int col = index / 3; | ||
477 | |||
478 | return GetElement(mat, row, col); | ||
479 | } | ||
480 | |||
481 | internal static float GetElement(Matrix mat, int row, int col) | ||
482 | { | ||
483 | switch (row) | ||
484 | { | ||
485 | case 0: | ||
486 | switch (col) | ||
487 | { | ||
488 | case 0: | ||
489 | return mat.M11; | ||
490 | case 1: | ||
491 | return mat.M12; | ||
492 | case 2: | ||
493 | return mat.M13; | ||
494 | } break; | ||
495 | case 1: | ||
496 | switch (col) | ||
497 | { | ||
498 | case 0: | ||
499 | return mat.M21; | ||
500 | case 1: | ||
501 | return mat.M22; | ||
502 | case 2: | ||
503 | return mat.M23; | ||
504 | } break; | ||
505 | case 2: | ||
506 | switch (col) | ||
507 | { | ||
508 | case 0: | ||
509 | return mat.M31; | ||
510 | case 1: | ||
511 | return mat.M32; | ||
512 | case 2: | ||
513 | return mat.M33; | ||
514 | } break; | ||
515 | } | ||
516 | |||
517 | return 0; | ||
518 | } | ||
519 | |||
520 | internal static float GetElement(Vector3 v, int index) | ||
521 | { | ||
522 | if (index == 0) | ||
523 | return v.X; | ||
524 | if (index == 1) | ||
525 | return v.Y; | ||
526 | if (index == 2) | ||
527 | return v.Z; | ||
528 | |||
529 | throw new ArgumentOutOfRangeException("index"); | ||
530 | } | ||
531 | |||
532 | internal static void SetElement(ref Vector3 v, int index, float value) | ||
533 | { | ||
534 | if (index == 0) | ||
535 | v.X = value; | ||
536 | else if (index == 1) | ||
537 | v.Y = value; | ||
538 | else if (index == 2) | ||
539 | v.Z = value; | ||
540 | else | ||
541 | throw new ArgumentOutOfRangeException("index"); | ||
542 | } | ||
543 | |||
544 | public static Matrix InvertMatrix(Matrix m) | ||
545 | { | ||
546 | Vector3 pos = m.Translation; | ||
547 | m.Translation = Vector3.Zero; | ||
548 | Matrix inv = Matrix.Transpose(m); | ||
549 | pos = Vector3.Transform(-pos, m); | ||
550 | inv.Translation = pos; | ||
551 | return inv; | ||
552 | } | ||
553 | |||
554 | public static Matrix GetDisplayMatrix(Matrix m) | ||
555 | { | ||
556 | Matrix displayMatrix = m; | ||
557 | displayMatrix.Translation = Vector3.Zero; | ||
558 | displayMatrix = Matrix.Transpose(displayMatrix); | ||
559 | displayMatrix.Translation = m.Translation; | ||
560 | return displayMatrix; | ||
561 | } | ||
562 | |||
563 | internal static Vector3 Transform(Vector3 position, Matrix matrix) | ||
564 | { | ||
565 | Vector3 vector = new Vector3(); | ||
566 | vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41; | ||
567 | vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42; | ||
568 | vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43; | ||
569 | return vector; | ||
570 | } | ||
571 | |||
572 | internal static Vector3 TransformNormal(Vector3 position, Matrix matrix) | ||
573 | { | ||
574 | Vector3 vector = new Vector3(); | ||
575 | vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)); | ||
576 | vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)); | ||
577 | vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)); | ||
578 | return vector; | ||
579 | } | ||
580 | } | ||
581 | } | ||