aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs214
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
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace 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
generated by cgit v1.1 at 2024-08-26 17:20:29 +0000