diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs | 115 |
1 files changed, 115 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs new file mode 100644 index 0000000..c127460 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs | |||
@@ -0,0 +1,115 @@ | |||
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 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class TriangleRaycastCallback : ITriangleCallback | ||
30 | { | ||
31 | private Vector3 _from; | ||
32 | private Vector3 _to; | ||
33 | private float _hitFraction; | ||
34 | |||
35 | public TriangleRaycastCallback(Vector3 from, Vector3 to) | ||
36 | { | ||
37 | _from = from; | ||
38 | _to = to; | ||
39 | _hitFraction = 1; | ||
40 | } | ||
41 | |||
42 | public Vector3 From { get { return _from; } set { _from = value; } } | ||
43 | public Vector3 To { get { return _to; } set { _to = value; } } | ||
44 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
45 | |||
46 | public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex); | ||
47 | |||
48 | #region ITriangleCallback Members | ||
49 | |||
50 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
51 | { | ||
52 | Vector3 vertA = triangle[0]; | ||
53 | Vector3 vertB = triangle[1]; | ||
54 | Vector3 vertC = triangle[2]; | ||
55 | |||
56 | Vector3 vBA = vertB - vertA; | ||
57 | Vector3 vCA = vertC - vertA; | ||
58 | |||
59 | Vector3 triangleNormal = Vector3.Cross(vBA, vCA); | ||
60 | |||
61 | float dist = Vector3.Dot(vertA, triangleNormal); | ||
62 | float distA = Vector3.Dot(triangleNormal, _from); | ||
63 | distA -= dist; | ||
64 | float distB = Vector3.Dot(triangleNormal, _to); | ||
65 | distB -= dist; | ||
66 | |||
67 | if (distA * distB >= 0.0f) | ||
68 | { | ||
69 | return; // same sign | ||
70 | } | ||
71 | |||
72 | float projLength = distA - distB; | ||
73 | float distance = (distA) / (projLength); | ||
74 | // Now we have the intersection point on the plane, we'll see if it's inside the triangle | ||
75 | // Add an epsilon as a tolerance for the raycast, | ||
76 | // in case the ray hits exacly on the edge of the triangle. | ||
77 | // It must be scaled for the triangle size. | ||
78 | |||
79 | if (distance < _hitFraction) | ||
80 | { | ||
81 | float edgeTolerance = triangleNormal.LengthSquared(); | ||
82 | edgeTolerance *= -0.0001f; | ||
83 | Vector3 point = new Vector3(); | ||
84 | MathHelper.SetInterpolate3(_from, _to, distance, ref point); | ||
85 | |||
86 | Vector3 vertexAPoint = vertA - point; | ||
87 | Vector3 vertexBPoint = vertB - point; | ||
88 | Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint); | ||
89 | |||
90 | if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance) | ||
91 | { | ||
92 | Vector3 vertexCPoint = vertC - point; | ||
93 | Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint); | ||
94 | if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance) | ||
95 | { | ||
96 | Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint); | ||
97 | |||
98 | if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance) | ||
99 | { | ||
100 | if (distA > 0) | ||
101 | { | ||
102 | _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex); | ||
103 | } | ||
104 | else | ||
105 | { | ||
106 | _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex); | ||
107 | } | ||
108 | } | ||
109 | } | ||
110 | } | ||
111 | } | ||
112 | } | ||
113 | #endregion | ||
114 | } | ||
115 | } | ||