diff options
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs | 202 |
1 files changed, 101 insertions, 101 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs index aa9d61e..20af192 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs | |||
@@ -1,101 +1,101 @@ | |||
1 | /* | 1 | /* |
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | 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 | 3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com |
4 | 4 | ||
5 | This software is provided 'as-is', without any express or implied | 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 | 6 | warranty. In no event will the authors be held liable for any damages |
7 | arising from the use of this software. | 7 | arising from the use of this software. |
8 | 8 | ||
9 | Permission is granted to anyone to use this software for any purpose, | 9 | Permission is granted to anyone to use this software for any purpose, |
10 | including commercial applications, and to alter it and redistribute it | 10 | including commercial applications, and to alter it and redistribute it |
11 | freely, subject to the following restrictions: | 11 | freely, subject to the following restrictions: |
12 | 12 | ||
13 | 1. The origin of this software must not be misrepresented; you must not | 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 | 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 | 15 | in a product, an acknowledgment in the product documentation would be |
16 | appreciated but is not required. | 16 | appreciated but is not required. |
17 | 2. Altered source versions must be plainly marked as such, and must not be | 17 | 2. Altered source versions must be plainly marked as such, and must not be |
18 | misrepresented as being the original software. | 18 | misrepresented as being the original software. |
19 | 3. This notice may not be removed or altered from any source distribution. | 19 | 3. This notice may not be removed or altered from any source distribution. |
20 | */ | 20 | */ |
21 | 21 | ||
22 | using System; | 22 | using System; |
23 | using System.Collections.Generic; | 23 | using System.Collections.Generic; |
24 | using System.Text; | 24 | using System.Text; |
25 | using MonoXnaCompactMaths; | 25 | using MonoXnaCompactMaths; |
26 | 26 | ||
27 | namespace XnaDevRu.BulletX | 27 | namespace XnaDevRu.BulletX |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// GjkEpaSolver contributed under zlib by Nathanael Presson | 30 | /// GjkEpaSolver contributed under zlib by Nathanael Presson |
31 | /// </summary> | 31 | /// </summary> |
32 | public class GjkEpaSolver | 32 | public class GjkEpaSolver |
33 | { | 33 | { |
34 | public struct Results | 34 | public struct Results |
35 | { | 35 | { |
36 | public enum Status | 36 | public enum Status |
37 | { | 37 | { |
38 | Separated, /* Shapes doesnt penetrate */ | 38 | Separated, /* Shapes doesnt penetrate */ |
39 | Penetrating, /* Shapes are penetrating */ | 39 | Penetrating, /* Shapes are penetrating */ |
40 | GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ | 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 */ | 41 | EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ |
42 | } | 42 | } |
43 | 43 | ||
44 | private Vector3[] _witnesses; | 44 | private Vector3[] _witnesses; |
45 | private Vector3 _normal; | 45 | private Vector3 _normal; |
46 | private float _depth; | 46 | private float _depth; |
47 | private int _epaIterations; | 47 | private int _epaIterations; |
48 | private int _gjkIterations; | 48 | private int _gjkIterations; |
49 | private Status _status; | 49 | private Status _status; |
50 | 50 | ||
51 | public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } | 51 | public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } |
52 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } | 52 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } |
53 | public float Depth { get { return _depth; } set { _depth = value; } } | 53 | public float Depth { get { return _depth; } set { _depth = value; } } |
54 | public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } | 54 | public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } |
55 | public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } | 55 | public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } |
56 | public Status ResultStatus { get { return _status; } set { _status = value; } } | 56 | public Status ResultStatus { get { return _status; } set { _status = value; } } |
57 | } | 57 | } |
58 | 58 | ||
59 | public static bool Collide(ConvexShape shapeA, Matrix wtrsA, | 59 | public static bool Collide(ConvexShape shapeA, Matrix wtrsA, |
60 | ConvexShape shapeB, Matrix wtrsB, | 60 | ConvexShape shapeB, Matrix wtrsB, |
61 | float radialmargin, | 61 | float radialmargin, |
62 | out Results results) | 62 | out Results results) |
63 | { | 63 | { |
64 | /* Initialize */ | 64 | /* Initialize */ |
65 | results = new Results(); | 65 | results = new Results(); |
66 | results.Witnesses = new Vector3[2]; | 66 | results.Witnesses = new Vector3[2]; |
67 | results.Witnesses[0] = | 67 | results.Witnesses[0] = |
68 | results.Witnesses[1] = | 68 | results.Witnesses[1] = |
69 | results.Normal = new Vector3(); | 69 | results.Normal = new Vector3(); |
70 | results.Depth = 0; | 70 | results.Depth = 0; |
71 | results.ResultStatus = Results.Status.Separated; | 71 | results.ResultStatus = Results.Status.Separated; |
72 | results.EpaIterations = 0; | 72 | results.EpaIterations = 0; |
73 | results.GjkIterations = 0; | 73 | results.GjkIterations = 0; |
74 | /* Use GJK to locate origin */ | 74 | /* Use GJK to locate origin */ |
75 | GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, | 75 | GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, |
76 | wtrsB, wtrsB.Translation, shapeB, | 76 | wtrsB, wtrsB.Translation, shapeB, |
77 | radialmargin + GjkEpa.EpaAccuracy); | 77 | radialmargin + GjkEpa.EpaAccuracy); |
78 | bool collide = gjk.SearchOrigin(); | 78 | bool collide = gjk.SearchOrigin(); |
79 | results.GjkIterations = gjk.Iterations + 1; | 79 | results.GjkIterations = gjk.Iterations + 1; |
80 | if (collide) | 80 | if (collide) |
81 | { | 81 | { |
82 | /* Then EPA for penetration depth */ | 82 | /* Then EPA for penetration depth */ |
83 | GjkEpa.Epa epa = new GjkEpa.Epa(gjk); | 83 | GjkEpa.Epa epa = new GjkEpa.Epa(gjk); |
84 | float pd = epa.EvaluatePD(); | 84 | float pd = epa.EvaluatePD(); |
85 | results.EpaIterations = epa.Iterations + 1; | 85 | results.EpaIterations = epa.Iterations + 1; |
86 | if (pd > 0) | 86 | if (pd > 0) |
87 | { | 87 | { |
88 | results.ResultStatus = Results.Status.Penetrating; | 88 | results.ResultStatus = Results.Status.Penetrating; |
89 | results.Normal = epa.Normal; | 89 | results.Normal = epa.Normal; |
90 | results.Depth = pd; | 90 | results.Depth = pd; |
91 | results.Witnesses[0] = epa.Nearest[0]; | 91 | results.Witnesses[0] = epa.Nearest[0]; |
92 | results.Witnesses[1] = epa.Nearest[1]; | 92 | results.Witnesses[1] = epa.Nearest[1]; |
93 | return true; | 93 | return true; |
94 | } | 94 | } |
95 | else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } | 95 | else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } |
96 | } | 96 | } |
97 | else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } | 97 | else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } |
98 | return false; | 98 | return false; |
99 | } | 99 | } |
100 | } | 100 | } |
101 | } | 101 | } |