diff options
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs | 101 |
1 files changed, 101 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs new file mode 100644 index 0000000..aa9d61e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs | |||
@@ -0,0 +1,101 @@ | |||
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 | /// <summary> | ||
30 | /// GjkEpaSolver contributed under zlib by Nathanael Presson | ||
31 | /// </summary> | ||
32 | public class GjkEpaSolver | ||
33 | { | ||
34 | public struct Results | ||
35 | { | ||
36 | public enum Status | ||
37 | { | ||
38 | Separated, /* Shapes doesnt penetrate */ | ||
39 | Penetrating, /* Shapes are penetrating */ | ||
40 | GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ | ||
41 | EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ | ||
42 | } | ||
43 | |||
44 | private Vector3[] _witnesses; | ||
45 | private Vector3 _normal; | ||
46 | private float _depth; | ||
47 | private int _epaIterations; | ||
48 | private int _gjkIterations; | ||
49 | private Status _status; | ||
50 | |||
51 | public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } | ||
52 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } | ||
53 | public float Depth { get { return _depth; } set { _depth = value; } } | ||
54 | public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } | ||
55 | public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } | ||
56 | public Status ResultStatus { get { return _status; } set { _status = value; } } | ||
57 | } | ||
58 | |||
59 | public static bool Collide(ConvexShape shapeA, Matrix wtrsA, | ||
60 | ConvexShape shapeB, Matrix wtrsB, | ||
61 | float radialmargin, | ||
62 | out Results results) | ||
63 | { | ||
64 | /* Initialize */ | ||
65 | results = new Results(); | ||
66 | results.Witnesses = new Vector3[2]; | ||
67 | results.Witnesses[0] = | ||
68 | results.Witnesses[1] = | ||
69 | results.Normal = new Vector3(); | ||
70 | results.Depth = 0; | ||
71 | results.ResultStatus = Results.Status.Separated; | ||
72 | results.EpaIterations = 0; | ||
73 | results.GjkIterations = 0; | ||
74 | /* Use GJK to locate origin */ | ||
75 | GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, | ||
76 | wtrsB, wtrsB.Translation, shapeB, | ||
77 | radialmargin + GjkEpa.EpaAccuracy); | ||
78 | bool collide = gjk.SearchOrigin(); | ||
79 | results.GjkIterations = gjk.Iterations + 1; | ||
80 | if (collide) | ||
81 | { | ||
82 | /* Then EPA for penetration depth */ | ||
83 | GjkEpa.Epa epa = new GjkEpa.Epa(gjk); | ||
84 | float pd = epa.EvaluatePD(); | ||
85 | results.EpaIterations = epa.Iterations + 1; | ||
86 | if (pd > 0) | ||
87 | { | ||
88 | results.ResultStatus = Results.Status.Penetrating; | ||
89 | results.Normal = epa.Normal; | ||
90 | results.Depth = pd; | ||
91 | results.Witnesses[0] = epa.Nearest[0]; | ||
92 | results.Witnesses[1] = epa.Nearest[1]; | ||
93 | return true; | ||
94 | } | ||
95 | else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } | ||
96 | } | ||
97 | else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } | ||
98 | return false; | ||
99 | } | ||
100 | } | ||
101 | } | ||