diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs | 214 |
1 files changed, 0 insertions, 214 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs deleted file mode 100644 index 4d578d8..0000000 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs +++ /dev/null | |||
@@ -1,214 +0,0 @@ | |||
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 class SphereTriangleDetector : DiscreteCollisionDetectorInterface | ||
30 | { | ||
31 | private SphereShape _sphere; | ||
32 | private TriangleShape _triangle; | ||
33 | private const int MaxOverlap = 0; | ||
34 | |||
35 | public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle) | ||
36 | { | ||
37 | this._sphere = sphere; | ||
38 | this._triangle = triangle; | ||
39 | } | ||
40 | |||
41 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) | ||
42 | { | ||
43 | Matrix transformA = input.TransformA; | ||
44 | Matrix transformB = input.TransformB; | ||
45 | |||
46 | Vector3 point = new Vector3(); | ||
47 | Vector3 normal = new Vector3(); | ||
48 | Single timeOfImpact = 1.0f; | ||
49 | Single depth = 0.0f; | ||
50 | |||
51 | //move sphere into triangle space | ||
52 | Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA); | ||
53 | |||
54 | if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact)) | ||
55 | output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth); | ||
56 | } | ||
57 | |||
58 | /// <summary> | ||
59 | /// See also geometrictools.com | ||
60 | /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv | ||
61 | /// </summary> | ||
62 | /// <param name="from"></param> | ||
63 | /// <param name="to"></param> | ||
64 | /// <param name="p"></param> | ||
65 | /// <param name="nearest"></param> | ||
66 | /// <returns></returns> | ||
67 | private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest) | ||
68 | { | ||
69 | Vector3 diff = point - from; | ||
70 | Vector3 v = to - from; | ||
71 | float t = Vector3.Dot(v, diff); | ||
72 | |||
73 | if (t > 0) | ||
74 | { | ||
75 | float dotVV = Vector3.Dot(v, v); | ||
76 | if (t < dotVV) | ||
77 | { | ||
78 | t /= dotVV; | ||
79 | diff -= t * v; | ||
80 | } | ||
81 | else | ||
82 | { | ||
83 | t = 1; | ||
84 | diff -= v; | ||
85 | } | ||
86 | } | ||
87 | else | ||
88 | t = 0; | ||
89 | |||
90 | nearest = from + t * v; | ||
91 | return Vector3.Dot(diff, diff); | ||
92 | } | ||
93 | |||
94 | private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact) | ||
95 | { | ||
96 | Vector3[] vertices = _triangle.Vertices; | ||
97 | Vector3 c = sphereCenter; | ||
98 | float r = _sphere.Radius; | ||
99 | |||
100 | Vector3 delta = new Vector3(); | ||
101 | |||
102 | Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); | ||
103 | normal = Vector3.Normalize(normal); | ||
104 | Vector3 p1ToCentre = c - vertices[0]; | ||
105 | float distanceFromPlane = Vector3.Dot(p1ToCentre, normal); | ||
106 | |||
107 | if (distanceFromPlane < 0) | ||
108 | { | ||
109 | //triangle facing the other way | ||
110 | distanceFromPlane *= -1; | ||
111 | normal *= -1; | ||
112 | } | ||
113 | |||
114 | float contactMargin = PersistentManifold.ContactBreakingThreshold; | ||
115 | bool isInsideContactPlane = distanceFromPlane < r + contactMargin; | ||
116 | bool isInsideShellPlane = distanceFromPlane < r; | ||
117 | |||
118 | float deltaDotNormal = Vector3.Dot(delta, normal); | ||
119 | if (!isInsideShellPlane && deltaDotNormal >= 0.0f) | ||
120 | return false; | ||
121 | |||
122 | // Check for contact / intersection | ||
123 | bool hasContact = false; | ||
124 | Vector3 contactPoint = new Vector3(); | ||
125 | if (isInsideContactPlane) | ||
126 | { | ||
127 | if (FaceContains(c, vertices, normal)) | ||
128 | { | ||
129 | // Inside the contact wedge - touches a point on the shell plane | ||
130 | hasContact = true; | ||
131 | contactPoint = c - normal * distanceFromPlane; | ||
132 | } | ||
133 | else | ||
134 | { | ||
135 | // Could be inside one of the contact capsules | ||
136 | float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); | ||
137 | Vector3 nearestOnEdge = new Vector3(); | ||
138 | for (int i = 0; i < _triangle.EdgeCount; i++) | ||
139 | { | ||
140 | Vector3 pa, pb; | ||
141 | _triangle.GetEdge(i, out pa, out pb); | ||
142 | |||
143 | float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge); | ||
144 | if (distanceSqr < contactCapsuleRadiusSqr) | ||
145 | { | ||
146 | // Yep, we're inside a capsule | ||
147 | hasContact = true; | ||
148 | contactPoint = nearestOnEdge; | ||
149 | } | ||
150 | } | ||
151 | } | ||
152 | } | ||
153 | |||
154 | if (hasContact) | ||
155 | { | ||
156 | Vector3 contactToCentre = c - contactPoint; | ||
157 | float distanceSqr = contactToCentre.LengthSquared(); | ||
158 | if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap)) | ||
159 | { | ||
160 | float distance = (float)Math.Sqrt(distanceSqr); | ||
161 | resultNormal = contactToCentre; | ||
162 | resultNormal = Vector3.Normalize(resultNormal); | ||
163 | point = contactPoint; | ||
164 | depth = -(r - distance); | ||
165 | return true; | ||
166 | } | ||
167 | |||
168 | if (Vector3.Dot(delta, contactToCentre) >= 0.0f) | ||
169 | return false; | ||
170 | |||
171 | // Moving towards the contact point -> collision | ||
172 | point = contactPoint; | ||
173 | timeOfImpact = 0.0f; | ||
174 | return true; | ||
175 | } | ||
176 | return false; | ||
177 | } | ||
178 | |||
179 | private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p) | ||
180 | { | ||
181 | Vector3 p1 = vertices[0]; | ||
182 | Vector3 p2 = vertices[1]; | ||
183 | Vector3 p3 = vertices[2]; | ||
184 | |||
185 | Vector3 edge1 = p2 - p1; | ||
186 | Vector3 edge2 = p3 - p2; | ||
187 | Vector3 edge3 = p1 - p3; | ||
188 | |||
189 | Vector3 p1ToP = p - p1; | ||
190 | Vector3 p2ToP = p - p2; | ||
191 | Vector3 p3ToP = p - p3; | ||
192 | |||
193 | Vector3 edge1Normal = Vector3.Cross(edge1, normal); | ||
194 | Vector3 edge2Normal = Vector3.Cross(edge2, normal); | ||
195 | Vector3 edge3Normal = Vector3.Cross(edge3, normal); | ||
196 | |||
197 | float r1, r2, r3; | ||
198 | r1 = Vector3.Dot(edge1Normal, p1ToP); | ||
199 | r2 = Vector3.Dot(edge2Normal, p2ToP); | ||
200 | r3 = Vector3.Dot(edge3Normal, p3ToP); | ||
201 | if ((r1 > 0 && r2 > 0 && r3 > 0) || | ||
202 | (r1 <= 0 && r2 <= 0 && r3 <= 0)) | ||
203 | return true; | ||
204 | return false; | ||
205 | } | ||
206 | |||
207 | private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal) | ||
208 | { | ||
209 | Vector3 lp = p; | ||
210 | Vector3 lnormal = normal; | ||
211 | return PointInTriangle(vertices, lnormal, lp); | ||
212 | } | ||
213 | } | ||
214 | } \ No newline at end of file | ||