diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs | 643 |
1 files changed, 643 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs new file mode 100644 index 0000000..16f3dab --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs | |||
@@ -0,0 +1,643 @@ | |||
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 UsageBitfield | ||
30 | { | ||
31 | private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD; | ||
32 | |||
33 | public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } } | ||
34 | public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } } | ||
35 | public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } } | ||
36 | public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } } | ||
37 | |||
38 | public void Reset() | ||
39 | { | ||
40 | _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false; | ||
41 | } | ||
42 | } | ||
43 | |||
44 | public class SubSimplexClosestResult | ||
45 | { | ||
46 | private Vector3 _closestPointOnSimplex; | ||
47 | |||
48 | //MASK for m_usedVertices | ||
49 | //stores the simplex vertex-usage, using the MASK, | ||
50 | // if m_usedVertices & MASK then the related vertex is used | ||
51 | private UsageBitfield _usedVertices = new UsageBitfield(); | ||
52 | private float[] _barycentricCoords = new float[4]; | ||
53 | private bool _degenerate; | ||
54 | |||
55 | public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } } | ||
56 | public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } } | ||
57 | public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } } | ||
58 | public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } } | ||
59 | |||
60 | public void Reset() | ||
61 | { | ||
62 | _degenerate = false; | ||
63 | SetBarycentricCoordinates(); | ||
64 | _usedVertices.Reset(); | ||
65 | } | ||
66 | |||
67 | public bool IsValid | ||
68 | { | ||
69 | get | ||
70 | { | ||
71 | return (_barycentricCoords[0] >= 0f) && | ||
72 | (_barycentricCoords[1] >= 0f) && | ||
73 | (_barycentricCoords[2] >= 0f) && | ||
74 | (_barycentricCoords[3] >= 0f); | ||
75 | } | ||
76 | } | ||
77 | |||
78 | public void SetBarycentricCoordinates() | ||
79 | { | ||
80 | SetBarycentricCoordinates(0f, 0f, 0f, 0f); | ||
81 | } | ||
82 | |||
83 | public void SetBarycentricCoordinates(float a, float b, float c, float d) | ||
84 | { | ||
85 | _barycentricCoords[0] = a; | ||
86 | _barycentricCoords[1] = b; | ||
87 | _barycentricCoords[2] = c; | ||
88 | _barycentricCoords[3] = d; | ||
89 | } | ||
90 | } | ||
91 | |||
92 | /// VoronoiSimplexSolver is an implementation of the closest point distance | ||
93 | /// algorithm from a 1-4 points simplex to the origin. | ||
94 | /// Can be used with GJK, as an alternative to Johnson distance algorithm. | ||
95 | public class VoronoiSimplexSolver : ISimplexSolver | ||
96 | { | ||
97 | private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3; | ||
98 | |||
99 | private const int VoronoiSimplexMaxVerts = 5; | ||
100 | private const bool CatchDegenerateTetrahedron = true; | ||
101 | |||
102 | private int _numVertices; | ||
103 | |||
104 | private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts]; | ||
105 | private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts]; | ||
106 | private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts]; | ||
107 | |||
108 | private Vector3 _cachedPA; | ||
109 | private Vector3 _cachedPB; | ||
110 | private Vector3 _cachedV; | ||
111 | private Vector3 _lastW; | ||
112 | private bool _cachedValidClosest; | ||
113 | |||
114 | private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult(); | ||
115 | |||
116 | private bool _needsUpdate; | ||
117 | |||
118 | #region ISimplexSolver Members | ||
119 | |||
120 | public bool FullSimplex | ||
121 | { | ||
122 | get | ||
123 | { | ||
124 | return _numVertices == 4; | ||
125 | } | ||
126 | } | ||
127 | |||
128 | public int NumVertices | ||
129 | { | ||
130 | get | ||
131 | { | ||
132 | return _numVertices; | ||
133 | } | ||
134 | } | ||
135 | |||
136 | public void Reset() | ||
137 | { | ||
138 | _cachedValidClosest = false; | ||
139 | _numVertices = 0; | ||
140 | _needsUpdate = true; | ||
141 | _lastW = new Vector3(1e30f, 1e30f, 1e30f); | ||
142 | _cachedBC.Reset(); | ||
143 | } | ||
144 | |||
145 | public void AddVertex(Vector3 w, Vector3 p, Vector3 q) | ||
146 | { | ||
147 | _lastW = w; | ||
148 | _needsUpdate = true; | ||
149 | |||
150 | _simplexVectorW[_numVertices] = w; | ||
151 | _simplexPointsP[_numVertices] = p; | ||
152 | _simplexPointsQ[_numVertices] = q; | ||
153 | |||
154 | _numVertices++; | ||
155 | } | ||
156 | |||
157 | //return/calculate the closest vertex | ||
158 | public bool Closest(out Vector3 v) | ||
159 | { | ||
160 | bool succes = UpdateClosestVectorAndPoints(); | ||
161 | v = _cachedV; | ||
162 | return succes; | ||
163 | } | ||
164 | |||
165 | public float MaxVertex | ||
166 | { | ||
167 | get | ||
168 | { | ||
169 | int numverts = NumVertices; | ||
170 | float maxV = 0f, curLen2; | ||
171 | for (int i = 0; i < numverts; i++) | ||
172 | { | ||
173 | curLen2 = _simplexVectorW[i].LengthSquared(); | ||
174 | if (maxV < curLen2) maxV = curLen2; | ||
175 | } | ||
176 | return maxV; | ||
177 | } | ||
178 | } | ||
179 | |||
180 | //return the current simplex | ||
181 | public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf) | ||
182 | { | ||
183 | int numverts = NumVertices; | ||
184 | pBuf = new Vector3[numverts]; | ||
185 | qBuf = new Vector3[numverts]; | ||
186 | yBuf = new Vector3[numverts]; | ||
187 | for (int i = 0; i < numverts; i++) | ||
188 | { | ||
189 | yBuf[i] = _simplexVectorW[i]; | ||
190 | pBuf[i] = _simplexPointsP[i]; | ||
191 | qBuf[i] = _simplexPointsQ[i]; | ||
192 | } | ||
193 | return numverts; | ||
194 | } | ||
195 | |||
196 | public bool InSimplex(Vector3 w) | ||
197 | { | ||
198 | //check in case lastW is already removed | ||
199 | if (w == _lastW) return true; | ||
200 | |||
201 | //w is in the current (reduced) simplex | ||
202 | int numverts = NumVertices; | ||
203 | for (int i = 0; i < numverts; i++) | ||
204 | if (_simplexVectorW[i] == w) return true; | ||
205 | |||
206 | return false; | ||
207 | } | ||
208 | |||
209 | public void BackupClosest(out Vector3 v) | ||
210 | { | ||
211 | v = _cachedV; | ||
212 | } | ||
213 | |||
214 | public bool EmptySimplex | ||
215 | { | ||
216 | get | ||
217 | { | ||
218 | return NumVertices == 0; | ||
219 | } | ||
220 | } | ||
221 | |||
222 | public void ComputePoints(out Vector3 p1, out Vector3 p2) | ||
223 | { | ||
224 | UpdateClosestVectorAndPoints(); | ||
225 | p1 = _cachedPA; | ||
226 | p2 = _cachedPB; | ||
227 | } | ||
228 | |||
229 | #endregion | ||
230 | |||
231 | public void RemoveVertex(int index) | ||
232 | { | ||
233 | BulletDebug.Assert(_numVertices > 0); | ||
234 | _numVertices--; | ||
235 | _simplexVectorW[index] = _simplexVectorW[_numVertices]; | ||
236 | _simplexPointsP[index] = _simplexPointsP[_numVertices]; | ||
237 | _simplexPointsQ[index] = _simplexPointsQ[_numVertices]; | ||
238 | } | ||
239 | |||
240 | public void ReduceVertices(UsageBitfield usedVerts) | ||
241 | { | ||
242 | if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3); | ||
243 | if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2); | ||
244 | if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1); | ||
245 | if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0); | ||
246 | } | ||
247 | |||
248 | public bool UpdateClosestVectorAndPoints() | ||
249 | { | ||
250 | if (_needsUpdate) | ||
251 | { | ||
252 | _cachedBC.Reset(); | ||
253 | _needsUpdate = false; | ||
254 | |||
255 | Vector3 p, a, b, c, d; | ||
256 | switch (NumVertices) | ||
257 | { | ||
258 | case 0: | ||
259 | _cachedValidClosest = false; | ||
260 | break; | ||
261 | case 1: | ||
262 | _cachedPA = _simplexPointsP[0]; | ||
263 | _cachedPB = _simplexPointsQ[0]; | ||
264 | _cachedV = _cachedPA - _cachedPB; | ||
265 | _cachedBC.Reset(); | ||
266 | _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f); | ||
267 | _cachedValidClosest = _cachedBC.IsValid; | ||
268 | break; | ||
269 | case 2: | ||
270 | //closest point origin from line segment | ||
271 | Vector3 from = _simplexVectorW[0]; | ||
272 | Vector3 to = _simplexVectorW[1]; | ||
273 | Vector3 nearest; | ||
274 | |||
275 | Vector3 diff = -from; | ||
276 | Vector3 v = to - from; | ||
277 | float t = Vector3.Dot(v, diff); | ||
278 | |||
279 | if (t > 0) | ||
280 | { | ||
281 | float dotVV = v.LengthSquared(); | ||
282 | if (t < dotVV) | ||
283 | { | ||
284 | t /= dotVV; | ||
285 | diff -= t * v; | ||
286 | _cachedBC.UsedVertices.UsedVertexA = true; | ||
287 | _cachedBC.UsedVertices.UsedVertexB = true; | ||
288 | } | ||
289 | else | ||
290 | { | ||
291 | t = 1; | ||
292 | diff -= v; | ||
293 | //reduce to 1 point | ||
294 | _cachedBC.UsedVertices.UsedVertexB = true; | ||
295 | } | ||
296 | } | ||
297 | else | ||
298 | { | ||
299 | t = 0; | ||
300 | //reduce to 1 point | ||
301 | _cachedBC.UsedVertices.UsedVertexA = true; | ||
302 | } | ||
303 | |||
304 | _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0); | ||
305 | nearest = from + t * v; | ||
306 | |||
307 | _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); | ||
308 | _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); | ||
309 | _cachedV = _cachedPA - _cachedPB; | ||
310 | |||
311 | ReduceVertices(_cachedBC.UsedVertices); | ||
312 | |||
313 | _cachedValidClosest = _cachedBC.IsValid; | ||
314 | break; | ||
315 | case 3: | ||
316 | //closest point origin from triangle | ||
317 | p = new Vector3(); | ||
318 | a = _simplexVectorW[0]; | ||
319 | b = _simplexVectorW[1]; | ||
320 | c = _simplexVectorW[2]; | ||
321 | |||
322 | ClosestPtPointTriangle(p, a, b, c, ref _cachedBC); | ||
323 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + | ||
324 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + | ||
325 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + | ||
326 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; | ||
327 | |||
328 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + | ||
329 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + | ||
330 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + | ||
331 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; | ||
332 | |||
333 | _cachedV = _cachedPA - _cachedPB; | ||
334 | |||
335 | ReduceVertices(_cachedBC.UsedVertices); | ||
336 | _cachedValidClosest = _cachedBC.IsValid; | ||
337 | break; | ||
338 | case 4: | ||
339 | p = new Vector3(); | ||
340 | a = _simplexVectorW[0]; | ||
341 | b = _simplexVectorW[1]; | ||
342 | c = _simplexVectorW[2]; | ||
343 | d = _simplexVectorW[3]; | ||
344 | |||
345 | bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC); | ||
346 | |||
347 | if (hasSeperation) | ||
348 | { | ||
349 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + | ||
350 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + | ||
351 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + | ||
352 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; | ||
353 | |||
354 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + | ||
355 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + | ||
356 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + | ||
357 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; | ||
358 | |||
359 | _cachedV = _cachedPA - _cachedPB; | ||
360 | ReduceVertices(_cachedBC.UsedVertices); | ||
361 | } | ||
362 | else | ||
363 | { | ||
364 | if (_cachedBC.Degenerate) | ||
365 | { | ||
366 | _cachedValidClosest = false; | ||
367 | } | ||
368 | else | ||
369 | { | ||
370 | _cachedValidClosest = true; | ||
371 | //degenerate case == false, penetration = true + zero | ||
372 | _cachedV.X = _cachedV.Y = _cachedV.Z = 0f; | ||
373 | } | ||
374 | break; // !!!!!!!!!!!! proverit na vsakiy sluchai | ||
375 | } | ||
376 | |||
377 | _cachedValidClosest = _cachedBC.IsValid; | ||
378 | |||
379 | //closest point origin from tetrahedron | ||
380 | break; | ||
381 | default: | ||
382 | _cachedValidClosest = false; | ||
383 | break; | ||
384 | } | ||
385 | } | ||
386 | |||
387 | return _cachedValidClosest; | ||
388 | } | ||
389 | |||
390 | public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, | ||
391 | ref SubSimplexClosestResult result) | ||
392 | { | ||
393 | result.UsedVertices.Reset(); | ||
394 | |||
395 | float v, w; | ||
396 | |||
397 | // Check if P in vertex region outside A | ||
398 | Vector3 ab = b - a; | ||
399 | Vector3 ac = c - a; | ||
400 | Vector3 ap = p - a; | ||
401 | float d1 = Vector3.Dot(ab, ap); | ||
402 | float d2 = Vector3.Dot(ac, ap); | ||
403 | if (d1 <= 0f && d2 <= 0f) | ||
404 | { | ||
405 | result.ClosestPointOnSimplex = a; | ||
406 | result.UsedVertices.UsedVertexA = true; | ||
407 | result.SetBarycentricCoordinates(1, 0, 0, 0); | ||
408 | return true; // a; // barycentric coordinates (1,0,0) | ||
409 | } | ||
410 | |||
411 | // Check if P in vertex region outside B | ||
412 | Vector3 bp = p - b; | ||
413 | float d3 = Vector3.Dot(ab, bp); | ||
414 | float d4 = Vector3.Dot(ac, bp); | ||
415 | if (d3 >= 0f && d4 <= d3) | ||
416 | { | ||
417 | result.ClosestPointOnSimplex = b; | ||
418 | result.UsedVertices.UsedVertexB = true; | ||
419 | result.SetBarycentricCoordinates(0, 1, 0, 0); | ||
420 | |||
421 | return true; // b; // barycentric coordinates (0,1,0) | ||
422 | } | ||
423 | // Check if P in edge region of AB, if so return projection of P onto AB | ||
424 | float vc = d1 * d4 - d3 * d2; | ||
425 | if (vc <= 0f && d1 >= 0f && d3 <= 0f) | ||
426 | { | ||
427 | v = d1 / (d1 - d3); | ||
428 | result.ClosestPointOnSimplex = a + v * ab; | ||
429 | result.UsedVertices.UsedVertexA = true; | ||
430 | result.UsedVertices.UsedVertexB = true; | ||
431 | result.SetBarycentricCoordinates(1 - v, v, 0, 0); | ||
432 | return true; | ||
433 | //return a + v * ab; // barycentric coordinates (1-v,v,0) | ||
434 | } | ||
435 | |||
436 | // Check if P in vertex region outside C | ||
437 | Vector3 cp = p - c; | ||
438 | float d5 = Vector3.Dot(ab, cp); | ||
439 | float d6 = Vector3.Dot(ac, cp); | ||
440 | if (d6 >= 0f && d5 <= d6) | ||
441 | { | ||
442 | result.ClosestPointOnSimplex = c; | ||
443 | result.UsedVertices.UsedVertexC = true; | ||
444 | result.SetBarycentricCoordinates(0, 0, 1, 0); | ||
445 | return true;//c; // barycentric coordinates (0,0,1) | ||
446 | } | ||
447 | |||
448 | // Check if P in edge region of AC, if so return projection of P onto AC | ||
449 | float vb = d5 * d2 - d1 * d6; | ||
450 | if (vb <= 0f && d2 >= 0f && d6 <= 0f) | ||
451 | { | ||
452 | w = d2 / (d2 - d6); | ||
453 | result.ClosestPointOnSimplex = a + w * ac; | ||
454 | result.UsedVertices.UsedVertexA = true; | ||
455 | result.UsedVertices.UsedVertexC = true; | ||
456 | result.SetBarycentricCoordinates(1 - w, 0, w, 0); | ||
457 | return true; | ||
458 | //return a + w * ac; // barycentric coordinates (1-w,0,w) | ||
459 | } | ||
460 | |||
461 | // Check if P in edge region of BC, if so return projection of P onto BC | ||
462 | float va = d3 * d6 - d5 * d4; | ||
463 | if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) | ||
464 | { | ||
465 | w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); | ||
466 | |||
467 | result.ClosestPointOnSimplex = b + w * (c - b); | ||
468 | result.UsedVertices.UsedVertexB = true; | ||
469 | result.UsedVertices.UsedVertexC = true; | ||
470 | result.SetBarycentricCoordinates(0, 1 - w, w, 0); | ||
471 | return true; | ||
472 | // return b + w * (c - b); // barycentric coordinates (0,1-w,w) | ||
473 | } | ||
474 | |||
475 | // P inside face region. Compute Q through its barycentric coordinates (u,v,w) | ||
476 | float denom = 1.0f / (va + vb + vc); | ||
477 | v = vb * denom; | ||
478 | w = vc * denom; | ||
479 | |||
480 | result.ClosestPointOnSimplex = a + ab * v + ac * w; | ||
481 | result.UsedVertices.UsedVertexA = true; | ||
482 | result.UsedVertices.UsedVertexB = true; | ||
483 | result.UsedVertices.UsedVertexC = true; | ||
484 | result.SetBarycentricCoordinates(1 - v - w, v, w, 0); | ||
485 | |||
486 | return true; | ||
487 | } | ||
488 | |||
489 | /// Test if point p and d lie on opposite sides of plane through abc | ||
490 | public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d) | ||
491 | { | ||
492 | Vector3 normal = Vector3.Cross(b - a, c - a); | ||
493 | |||
494 | float signp = Vector3.Dot(p - a, normal); // [AP AB AC] | ||
495 | float signd = Vector3.Dot(d - a, normal); // [AD AB AC] | ||
496 | |||
497 | if (CatchDegenerateTetrahedron) | ||
498 | if (signd * signd < (1e-4f * 1e-4f)) return -1; | ||
499 | |||
500 | // Points on opposite sides if expression signs are opposite | ||
501 | return signp * signd < 0f ? 1 : 0; | ||
502 | } | ||
503 | |||
504 | public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d, | ||
505 | ref SubSimplexClosestResult finalResult) | ||
506 | { | ||
507 | SubSimplexClosestResult tempResult = new SubSimplexClosestResult(); | ||
508 | |||
509 | // Start out assuming point inside all halfspaces, so closest to itself | ||
510 | finalResult.ClosestPointOnSimplex = p; | ||
511 | finalResult.UsedVertices.Reset(); | ||
512 | finalResult.UsedVertices.UsedVertexA = true; | ||
513 | finalResult.UsedVertices.UsedVertexB = true; | ||
514 | finalResult.UsedVertices.UsedVertexC = true; | ||
515 | finalResult.UsedVertices.UsedVertexD = true; | ||
516 | |||
517 | int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); | ||
518 | int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); | ||
519 | int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); | ||
520 | int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); | ||
521 | |||
522 | if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) | ||
523 | { | ||
524 | finalResult.Degenerate = true; | ||
525 | return false; | ||
526 | } | ||
527 | |||
528 | if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) | ||
529 | return false; | ||
530 | |||
531 | float bestSqDist = float.MaxValue; | ||
532 | // If point outside face abc then compute closest point on abc | ||
533 | if (pointOutsideABC != 0) | ||
534 | { | ||
535 | ClosestPtPointTriangle(p, a, b, c, ref tempResult); | ||
536 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
537 | |||
538 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
539 | // Update best closest point if (squared) distance is less than current best | ||
540 | if (sqDist < bestSqDist) | ||
541 | { | ||
542 | bestSqDist = sqDist; | ||
543 | finalResult.ClosestPointOnSimplex = q; | ||
544 | //convert result bitmask! | ||
545 | finalResult.UsedVertices.Reset(); | ||
546 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
547 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB; | ||
548 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; | ||
549 | finalResult.SetBarycentricCoordinates( | ||
550 | tempResult.BarycentricCoords[VertexA], | ||
551 | tempResult.BarycentricCoords[VertexB], | ||
552 | tempResult.BarycentricCoords[VertexC], | ||
553 | 0); | ||
554 | } | ||
555 | } | ||
556 | |||
557 | // Repeat test for face acd | ||
558 | if (pointOutsideACD != 0) | ||
559 | { | ||
560 | ClosestPtPointTriangle(p, a, c, d, ref tempResult); | ||
561 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
562 | //convert result bitmask! | ||
563 | |||
564 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
565 | if (sqDist < bestSqDist) | ||
566 | { | ||
567 | bestSqDist = sqDist; | ||
568 | finalResult.ClosestPointOnSimplex = q; | ||
569 | finalResult.UsedVertices.Reset(); | ||
570 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
571 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB; | ||
572 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC; | ||
573 | finalResult.SetBarycentricCoordinates( | ||
574 | tempResult.BarycentricCoords[VertexA], | ||
575 | 0, | ||
576 | tempResult.BarycentricCoords[VertexB], | ||
577 | tempResult.BarycentricCoords[VertexC]); | ||
578 | } | ||
579 | } | ||
580 | // Repeat test for face adb | ||
581 | |||
582 | if (pointOutsideADB != 0) | ||
583 | { | ||
584 | ClosestPtPointTriangle(p, a, d, b, ref tempResult); | ||
585 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
586 | //convert result bitmask! | ||
587 | |||
588 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
589 | if (sqDist < bestSqDist) | ||
590 | { | ||
591 | bestSqDist = sqDist; | ||
592 | finalResult.ClosestPointOnSimplex = q; | ||
593 | finalResult.UsedVertices.Reset(); | ||
594 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
595 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; | ||
596 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC; | ||
597 | finalResult.SetBarycentricCoordinates( | ||
598 | tempResult.BarycentricCoords[VertexA], | ||
599 | tempResult.BarycentricCoords[VertexC], | ||
600 | 0, | ||
601 | tempResult.BarycentricCoords[VertexB]); | ||
602 | |||
603 | } | ||
604 | } | ||
605 | // Repeat test for face bdc | ||
606 | |||
607 | if (pointOutsideBDC != 0) | ||
608 | { | ||
609 | ClosestPtPointTriangle(p, b, d, c, ref tempResult); | ||
610 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
611 | //convert result bitmask! | ||
612 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
613 | if (sqDist < bestSqDist) | ||
614 | { | ||
615 | bestSqDist = sqDist; | ||
616 | finalResult.ClosestPointOnSimplex = q; | ||
617 | finalResult.UsedVertices.Reset(); | ||
618 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA; | ||
619 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; | ||
620 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; | ||
621 | |||
622 | finalResult.SetBarycentricCoordinates( | ||
623 | 0, | ||
624 | tempResult.BarycentricCoords[VertexA], | ||
625 | tempResult.BarycentricCoords[VertexC], | ||
626 | tempResult.BarycentricCoords[VertexB]); | ||
627 | } | ||
628 | } | ||
629 | |||
630 | //help! we ended up full ! | ||
631 | |||
632 | if (finalResult.UsedVertices.UsedVertexA && | ||
633 | finalResult.UsedVertices.UsedVertexB && | ||
634 | finalResult.UsedVertices.UsedVertexC && | ||
635 | finalResult.UsedVertices.UsedVertexD) | ||
636 | { | ||
637 | return true; | ||
638 | } | ||
639 | |||
640 | return true; | ||
641 | } | ||
642 | } | ||
643 | } | ||