diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs | 1266 |
1 files changed, 633 insertions, 633 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs index 50ae7ab..a3064db 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs | |||
@@ -1,633 +1,633 @@ | |||
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 | /// GJK-EPA collision solver by Nathanael Presson | 30 | /// GJK-EPA collision solver by Nathanael Presson |
31 | /// Nov.2006 | 31 | /// Nov.2006 |
32 | /// </summary> | 32 | /// </summary> |
33 | public class GjkEpa | 33 | public class GjkEpa |
34 | { | 34 | { |
35 | //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; | 35 | //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; |
36 | 36 | ||
37 | private static readonly float _infinity = MathHelper.Infinity; | 37 | private static readonly float _infinity = MathHelper.Infinity; |
38 | //private static readonly float _pi = (float)Math.PI; | 38 | //private static readonly float _pi = (float)Math.PI; |
39 | private static readonly float _twoPi = (float)(Math.PI * 2); | 39 | private static readonly float _twoPi = (float)(Math.PI * 2); |
40 | 40 | ||
41 | private static readonly int _gjkMaxIterations = 128; | 41 | private static readonly int _gjkMaxIterations = 128; |
42 | private static readonly int _gjkHashSize = 1 << 6; | 42 | private static readonly int _gjkHashSize = 1 << 6; |
43 | private static readonly int _gjkHashMask = _gjkHashSize - 1; | 43 | private static readonly int _gjkHashMask = _gjkHashSize - 1; |
44 | private static readonly float _gjkInSimplexEpsilon = 0.0001f; | 44 | private static readonly float _gjkInSimplexEpsilon = 0.0001f; |
45 | private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; | 45 | private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; |
46 | 46 | ||
47 | private static readonly int _epaMaxIterations = 256; | 47 | private static readonly int _epaMaxIterations = 256; |
48 | private static readonly float _epaInFaceEpsilon = 0.01f; | 48 | private static readonly float _epaInFaceEpsilon = 0.01f; |
49 | private static readonly float _epaAccuracy = 0.001f; | 49 | private static readonly float _epaAccuracy = 0.001f; |
50 | 50 | ||
51 | public static float EpaAccuracy { get { return _epaAccuracy; } } | 51 | public static float EpaAccuracy { get { return _epaAccuracy; } } |
52 | 52 | ||
53 | private static float Abs(float v) { return (v < 0 ? -v : v); } | 53 | private static float Abs(float v) { return (v < 0 ? -v : v); } |
54 | private static float Sign(float v) { return (v < 0 ? -1 : 1); } | 54 | private static float Sign(float v) { return (v < 0 ? -1 : 1); } |
55 | 55 | ||
56 | static void Swap<T>(ref T a, ref T b) | 56 | static void Swap<T>(ref T a, ref T b) |
57 | { | 57 | { |
58 | T t = a; | 58 | T t = a; |
59 | a = b; | 59 | a = b; |
60 | b = t; | 60 | b = t; |
61 | } | 61 | } |
62 | 62 | ||
63 | public class Gjk | 63 | public class Gjk |
64 | { | 64 | { |
65 | public class MinkowskiVertice | 65 | public class MinkowskiVertice |
66 | { | 66 | { |
67 | private Vector3 _vertice; /* Minkowski vertice */ | 67 | private Vector3 _vertice; /* Minkowski vertice */ |
68 | private Vector3 _ray; /* Ray */ | 68 | private Vector3 _ray; /* Ray */ |
69 | 69 | ||
70 | public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } | 70 | public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } |
71 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } | 71 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } |
72 | } | 72 | } |
73 | public class He | 73 | public class He |
74 | { | 74 | { |
75 | private Vector3 _ray; | 75 | private Vector3 _ray; |
76 | private He _next; | 76 | private He _next; |
77 | 77 | ||
78 | public He Next { get { return _next; } set { _next = value; } } | 78 | public He Next { get { return _next; } set { _next = value; } } |
79 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } | 79 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } |
80 | } | 80 | } |
81 | 81 | ||
82 | private He[] _table = new He[_gjkHashSize]; | 82 | private He[] _table = new He[_gjkHashSize]; |
83 | private Matrix[] _wrotations = new Matrix[2]; | 83 | private Matrix[] _wrotations = new Matrix[2]; |
84 | private Vector3[] _positions = new Vector3[2]; | 84 | private Vector3[] _positions = new Vector3[2]; |
85 | private ConvexShape[] _shapes = new ConvexShape[2]; | 85 | private ConvexShape[] _shapes = new ConvexShape[2]; |
86 | private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; | 86 | private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; |
87 | private Vector3 _ray; | 87 | private Vector3 _ray; |
88 | private int _order; | 88 | private int _order; |
89 | private int _iterations; | 89 | private int _iterations; |
90 | private float _margin; | 90 | private float _margin; |
91 | private bool _failed; | 91 | private bool _failed; |
92 | 92 | ||
93 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, | 93 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, |
94 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) | 94 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) |
95 | : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } | 95 | : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } |
96 | 96 | ||
97 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, | 97 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, |
98 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, | 98 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, |
99 | float pmargin) | 99 | float pmargin) |
100 | { | 100 | { |
101 | for (int i = 0; i < _simplex.Length; i++) | 101 | for (int i = 0; i < _simplex.Length; i++) |
102 | _simplex[i] = new MinkowskiVertice(); | 102 | _simplex[i] = new MinkowskiVertice(); |
103 | 103 | ||
104 | for (int i = 0; i < _wrotations.Length; i++) | 104 | for (int i = 0; i < _wrotations.Length; i++) |
105 | _wrotations[i] = new Matrix(); | 105 | _wrotations[i] = new Matrix(); |
106 | 106 | ||
107 | for (int i = 0; i < _positions.Length; i++) | 107 | for (int i = 0; i < _positions.Length; i++) |
108 | _positions[i] = new Vector3(); | 108 | _positions[i] = new Vector3(); |
109 | 109 | ||
110 | _wrotations[0] = wrotationA; _positions[0] = positionA; | 110 | _wrotations[0] = wrotationA; _positions[0] = positionA; |
111 | _shapes[0] = shapeA; | 111 | _shapes[0] = shapeA; |
112 | _wrotations[0].Translation = Vector3.Zero; | 112 | _wrotations[0].Translation = Vector3.Zero; |
113 | _wrotations[1] = wrotationB; _positions[1] = positionB; | 113 | _wrotations[1] = wrotationB; _positions[1] = positionB; |
114 | _shapes[1] = shapeB; | 114 | _shapes[1] = shapeB; |
115 | _wrotations[1].Translation = Vector3.Zero; | 115 | _wrotations[1].Translation = Vector3.Zero; |
116 | //sablock = sa->BeginBlock(); | 116 | //sablock = sa->BeginBlock(); |
117 | _margin = pmargin; | 117 | _margin = pmargin; |
118 | _failed = false; | 118 | _failed = false; |
119 | } | 119 | } |
120 | 120 | ||
121 | public bool Failed { get { return _failed; } } | 121 | public bool Failed { get { return _failed; } } |
122 | public int Iterations { get { return _iterations; } } | 122 | public int Iterations { get { return _iterations; } } |
123 | public int Order { get { return _order; } } | 123 | public int Order { get { return _order; } } |
124 | public MinkowskiVertice[] Simplex { get { return _simplex; } } | 124 | public MinkowskiVertice[] Simplex { get { return _simplex; } } |
125 | 125 | ||
126 | public int Hash(Vector3 v) | 126 | public int Hash(Vector3 v) |
127 | { | 127 | { |
128 | int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); | 128 | int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); |
129 | return (h * 169639) & _gjkHashMask; | 129 | return (h * 169639) & _gjkHashMask; |
130 | } | 130 | } |
131 | 131 | ||
132 | public bool FetchSupport() | 132 | public bool FetchSupport() |
133 | { | 133 | { |
134 | int h = Hash(_ray); | 134 | int h = Hash(_ray); |
135 | He e = _table[h]; | 135 | He e = _table[h]; |
136 | while (e != null) | 136 | while (e != null) |
137 | { | 137 | { |
138 | if (e.Ray == _ray) | 138 | if (e.Ray == _ray) |
139 | { | 139 | { |
140 | --_order; | 140 | --_order; |
141 | return (false); | 141 | return (false); |
142 | } | 142 | } |
143 | else e = e.Next; | 143 | else e = e.Next; |
144 | } | 144 | } |
145 | e = new He(); | 145 | e = new He(); |
146 | e.Ray = _ray; | 146 | e.Ray = _ray; |
147 | e.Next = _table[h]; | 147 | e.Next = _table[h]; |
148 | _table[h] = e; | 148 | _table[h] = e; |
149 | Support(_ray, ref _simplex[++_order]); | 149 | Support(_ray, ref _simplex[++_order]); |
150 | return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); | 150 | return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); |
151 | } | 151 | } |
152 | 152 | ||
153 | public Vector3 LocalSupport(Vector3 d, int i) | 153 | public Vector3 LocalSupport(Vector3 d, int i) |
154 | { | 154 | { |
155 | Matrix m = _wrotations[i]; | 155 | Matrix m = _wrotations[i]; |
156 | m.Translation = Vector3.Zero; | 156 | m.Translation = Vector3.Zero; |
157 | Vector3 vtx = Vector3.TransformNormal(d, m); | 157 | Vector3 vtx = Vector3.TransformNormal(d, m); |
158 | Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); | 158 | Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); |
159 | return (result + _positions[i]); | 159 | return (result + _positions[i]); |
160 | } | 160 | } |
161 | 161 | ||
162 | public void Support(Vector3 d, ref MinkowskiVertice v) | 162 | public void Support(Vector3 d, ref MinkowskiVertice v) |
163 | { | 163 | { |
164 | v.Ray = d; | 164 | v.Ray = d; |
165 | v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; | 165 | v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; |
166 | } | 166 | } |
167 | 167 | ||
168 | public bool SolveSimplex2(Vector3 ao, Vector3 ab) | 168 | public bool SolveSimplex2(Vector3 ao, Vector3 ab) |
169 | { | 169 | { |
170 | if (Vector3.Dot(ab, ao) >= 0) | 170 | if (Vector3.Dot(ab, ao) >= 0) |
171 | { | 171 | { |
172 | Vector3 cabo = Vector3.Cross(ab, ao); | 172 | Vector3 cabo = Vector3.Cross(ab, ao); |
173 | if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) | 173 | if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) |
174 | { _ray = Vector3.Cross(cabo, ab); } | 174 | { _ray = Vector3.Cross(cabo, ab); } |
175 | else | 175 | else |
176 | { return true; } | 176 | { return true; } |
177 | } | 177 | } |
178 | else | 178 | else |
179 | { | 179 | { |
180 | _order = 0; | 180 | _order = 0; |
181 | _simplex[0].Ray = _simplex[1].Ray; | 181 | _simplex[0].Ray = _simplex[1].Ray; |
182 | _simplex[0].Vertice = _simplex[1].Vertice; | 182 | _simplex[0].Vertice = _simplex[1].Vertice; |
183 | 183 | ||
184 | _ray = ao; | 184 | _ray = ao; |
185 | } | 185 | } |
186 | return false; | 186 | return false; |
187 | } | 187 | } |
188 | 188 | ||
189 | public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) | 189 | public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) |
190 | { | 190 | { |
191 | return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); | 191 | return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); |
192 | } | 192 | } |
193 | 193 | ||
194 | public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) | 194 | public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) |
195 | { | 195 | { |
196 | if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) | 196 | if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) |
197 | { | 197 | { |
198 | _order = 1; | 198 | _order = 1; |
199 | _simplex[0].Vertice = _simplex[1].Vertice; | 199 | _simplex[0].Vertice = _simplex[1].Vertice; |
200 | _simplex[0].Ray = _simplex[1].Ray; | 200 | _simplex[0].Ray = _simplex[1].Ray; |
201 | 201 | ||
202 | _simplex[1].Vertice = _simplex[2].Vertice; | 202 | _simplex[1].Vertice = _simplex[2].Vertice; |
203 | _simplex[1].Ray = _simplex[2].Ray; | 203 | _simplex[1].Ray = _simplex[2].Ray; |
204 | 204 | ||
205 | return (SolveSimplex2(ao, ab)); | 205 | return (SolveSimplex2(ao, ab)); |
206 | } | 206 | } |
207 | else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) | 207 | else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) |
208 | { | 208 | { |
209 | _order = 1; | 209 | _order = 1; |
210 | _simplex[1].Vertice = _simplex[2].Vertice; | 210 | _simplex[1].Vertice = _simplex[2].Vertice; |
211 | _simplex[1].Ray = _simplex[2].Ray; | 211 | _simplex[1].Ray = _simplex[2].Ray; |
212 | 212 | ||
213 | return (SolveSimplex2(ao, ac)); | 213 | return (SolveSimplex2(ao, ac)); |
214 | } | 214 | } |
215 | else | 215 | else |
216 | { | 216 | { |
217 | float d = Vector3.Dot(cabc, ao); | 217 | float d = Vector3.Dot(cabc, ao); |
218 | if (Abs(d) > _gjkInSimplexEpsilon) | 218 | if (Abs(d) > _gjkInSimplexEpsilon) |
219 | { | 219 | { |
220 | if (d > 0) | 220 | if (d > 0) |
221 | { _ray = cabc; } | 221 | { _ray = cabc; } |
222 | else | 222 | else |
223 | { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); } | 223 | { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); } |
224 | return (false); | 224 | return (false); |
225 | } | 225 | } |
226 | else return (true); | 226 | else return (true); |
227 | } | 227 | } |
228 | } | 228 | } |
229 | 229 | ||
230 | public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) | 230 | public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) |
231 | { | 231 | { |
232 | Vector3 crs; | 232 | Vector3 crs; |
233 | if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) | 233 | if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) |
234 | { | 234 | { |
235 | _order = 2; | 235 | _order = 2; |
236 | _simplex[0].Vertice = _simplex[1].Vertice; | 236 | _simplex[0].Vertice = _simplex[1].Vertice; |
237 | _simplex[0].Ray = _simplex[1].Ray; | 237 | _simplex[0].Ray = _simplex[1].Ray; |
238 | 238 | ||
239 | _simplex[1].Vertice = _simplex[2].Vertice; | 239 | _simplex[1].Vertice = _simplex[2].Vertice; |
240 | _simplex[1].Ray = _simplex[2].Ray; | 240 | _simplex[1].Ray = _simplex[2].Ray; |
241 | 241 | ||
242 | _simplex[2].Vertice = _simplex[3].Vertice; | 242 | _simplex[2].Vertice = _simplex[3].Vertice; |
243 | _simplex[2].Ray = _simplex[3].Ray; | 243 | _simplex[2].Ray = _simplex[3].Ray; |
244 | 244 | ||
245 | return (SolveSimplex3a(ao, ab, ac, crs)); | 245 | return (SolveSimplex3a(ao, ab, ac, crs)); |
246 | } | 246 | } |
247 | else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) | 247 | else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) |
248 | { | 248 | { |
249 | _order = 2; | 249 | _order = 2; |
250 | _simplex[2].Vertice = _simplex[3].Vertice; | 250 | _simplex[2].Vertice = _simplex[3].Vertice; |
251 | _simplex[2].Ray = _simplex[3].Ray; | 251 | _simplex[2].Ray = _simplex[3].Ray; |
252 | 252 | ||
253 | return (SolveSimplex3a(ao, ac, ad, crs)); | 253 | return (SolveSimplex3a(ao, ac, ad, crs)); |
254 | } | 254 | } |
255 | else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) | 255 | else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) |
256 | { | 256 | { |
257 | _order = 2; | 257 | _order = 2; |
258 | 258 | ||
259 | _simplex[1].Vertice = _simplex[0].Vertice; | 259 | _simplex[1].Vertice = _simplex[0].Vertice; |
260 | _simplex[1].Ray = _simplex[0].Ray; | 260 | _simplex[1].Ray = _simplex[0].Ray; |
261 | 261 | ||
262 | _simplex[0].Vertice = _simplex[2].Vertice; | 262 | _simplex[0].Vertice = _simplex[2].Vertice; |
263 | _simplex[0].Ray = _simplex[2].Ray; | 263 | _simplex[0].Ray = _simplex[2].Ray; |
264 | 264 | ||
265 | _simplex[2].Vertice = _simplex[3].Vertice; | 265 | _simplex[2].Vertice = _simplex[3].Vertice; |
266 | _simplex[2].Ray = _simplex[3].Ray; | 266 | _simplex[2].Ray = _simplex[3].Ray; |
267 | 267 | ||
268 | return (SolveSimplex3a(ao, ad, ab, crs)); | 268 | return (SolveSimplex3a(ao, ad, ab, crs)); |
269 | } | 269 | } |
270 | else return (true); | 270 | else return (true); |
271 | } | 271 | } |
272 | 272 | ||
273 | public bool SearchOrigin() | 273 | public bool SearchOrigin() |
274 | { | 274 | { |
275 | return SearchOrigin(new Vector3(1, 0, 0)); | 275 | return SearchOrigin(new Vector3(1, 0, 0)); |
276 | } | 276 | } |
277 | 277 | ||
278 | public bool SearchOrigin(Vector3 initray) | 278 | public bool SearchOrigin(Vector3 initray) |
279 | { | 279 | { |
280 | _iterations = 0; | 280 | _iterations = 0; |
281 | unchecked | 281 | unchecked |
282 | { | 282 | { |
283 | _order = (int)(-1); | 283 | _order = (int)(-1); |
284 | } | 284 | } |
285 | _failed = false; | 285 | _failed = false; |
286 | _ray = Vector3.Normalize(initray); | 286 | _ray = Vector3.Normalize(initray); |
287 | 287 | ||
288 | //ClearMemory(table, sizeof(void*) * GJK_hashsize); | 288 | //ClearMemory(table, sizeof(void*) * GJK_hashsize); |
289 | for (int i = 0; i < _table.Length; i++) | 289 | for (int i = 0; i < _table.Length; i++) |
290 | _table[i] = null; | 290 | _table[i] = null; |
291 | FetchSupport(); | 291 | FetchSupport(); |
292 | _ray = -_simplex[0].Vertice; | 292 | _ray = -_simplex[0].Vertice; |
293 | for (; _iterations < _gjkMaxIterations; ++_iterations) | 293 | for (; _iterations < _gjkMaxIterations; ++_iterations) |
294 | { | 294 | { |
295 | float rl = _ray.Length(); | 295 | float rl = _ray.Length(); |
296 | _ray /= rl > 0 ? rl : 1; | 296 | _ray /= rl > 0 ? rl : 1; |
297 | if (FetchSupport()) | 297 | if (FetchSupport()) |
298 | { | 298 | { |
299 | bool found = (false); | 299 | bool found = (false); |
300 | switch (_order) | 300 | switch (_order) |
301 | { | 301 | { |
302 | case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break; | 302 | case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break; |
303 | case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break; | 303 | case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break; |
304 | case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break; | 304 | case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break; |
305 | } | 305 | } |
306 | if (found) return (true); | 306 | if (found) return (true); |
307 | } | 307 | } |
308 | else return (false); | 308 | else return (false); |
309 | } | 309 | } |
310 | _failed = true; | 310 | _failed = true; |
311 | return (false); | 311 | return (false); |
312 | } | 312 | } |
313 | 313 | ||
314 | public bool EncloseOrigin() | 314 | public bool EncloseOrigin() |
315 | { | 315 | { |
316 | switch (_order) | 316 | switch (_order) |
317 | { | 317 | { |
318 | /* Point */ | 318 | /* Point */ |
319 | case 0: break; | 319 | case 0: break; |
320 | /* Line */ | 320 | /* Line */ |
321 | case 1: | 321 | case 1: |
322 | Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; | 322 | Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; |
323 | Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), | 323 | Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), |
324 | Vector3.Cross(ab, new Vector3(0, 1, 0)), | 324 | Vector3.Cross(ab, new Vector3(0, 1, 0)), |
325 | Vector3.Cross(ab, new Vector3(0, 0, 1)) }; | 325 | Vector3.Cross(ab, new Vector3(0, 0, 1)) }; |
326 | float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; | 326 | float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; |
327 | Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3)); | 327 | Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3)); |
328 | Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]; | 328 | Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]; |
329 | Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r); | 329 | Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r); |
330 | Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r); | 330 | Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r); |
331 | Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r); | 331 | Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r); |
332 | _order = 4; | 332 | _order = 4; |
333 | return true; | 333 | return true; |
334 | /* Triangle */ | 334 | /* Triangle */ |
335 | case 2: | 335 | case 2: |
336 | Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice)); | 336 | Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice)); |
337 | Support(n, ref _simplex[3]); | 337 | Support(n, ref _simplex[3]); |
338 | Support(-n, ref _simplex[4]); | 338 | Support(-n, ref _simplex[4]); |
339 | _order = 4; | 339 | _order = 4; |
340 | return true; | 340 | return true; |
341 | /* Tetrahedron */ | 341 | /* Tetrahedron */ |
342 | case 3: return (true); | 342 | case 3: return (true); |
343 | /* Hexahedron */ | 343 | /* Hexahedron */ |
344 | case 4: return (true); | 344 | case 4: return (true); |
345 | } | 345 | } |
346 | return (false); | 346 | return (false); |
347 | } | 347 | } |
348 | } | 348 | } |
349 | 349 | ||
350 | public class Epa | 350 | public class Epa |
351 | { | 351 | { |
352 | public class Face | 352 | public class Face |
353 | { | 353 | { |
354 | public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; | 354 | public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; |
355 | public Face[] _faces = new Face[3]; | 355 | public Face[] _faces = new Face[3]; |
356 | public int[] _e = new int[3]; | 356 | public int[] _e = new int[3]; |
357 | public Vector3 _n; | 357 | public Vector3 _n; |
358 | public float _d; | 358 | public float _d; |
359 | public int _mark; | 359 | public int _mark; |
360 | public Face _prev; | 360 | public Face _prev; |
361 | public Face _next; | 361 | public Face _next; |
362 | } | 362 | } |
363 | 363 | ||
364 | private Gjk _gjk; | 364 | private Gjk _gjk; |
365 | private Face _root; | 365 | private Face _root; |
366 | private int _nfaces; | 366 | private int _nfaces; |
367 | private int _iterations; | 367 | private int _iterations; |
368 | private Vector3[,] _features = new Vector3[2, 3]; | 368 | private Vector3[,] _features = new Vector3[2, 3]; |
369 | private Vector3[] _nearest = new Vector3[2]; | 369 | private Vector3[] _nearest = new Vector3[2]; |
370 | private Vector3 _normal; | 370 | private Vector3 _normal; |
371 | private float _depth; | 371 | private float _depth; |
372 | private bool _failed; | 372 | private bool _failed; |
373 | 373 | ||
374 | public Epa(Gjk gjk) | 374 | public Epa(Gjk gjk) |
375 | { | 375 | { |
376 | this._gjk = gjk; | 376 | this._gjk = gjk; |
377 | } | 377 | } |
378 | 378 | ||
379 | public bool Failed { get { return _failed; } } | 379 | public bool Failed { get { return _failed; } } |
380 | public int Iterations { get { return _iterations; } } | 380 | public int Iterations { get { return _iterations; } } |
381 | public Vector3 Normal { get { return _normal; } } | 381 | public Vector3 Normal { get { return _normal; } } |
382 | public Vector3[] Nearest { get { return _nearest; } } | 382 | public Vector3[] Nearest { get { return _nearest; } } |
383 | 383 | ||
384 | public Vector3 GetCoordinates(Face face) | 384 | public Vector3 GetCoordinates(Face face) |
385 | { | 385 | { |
386 | Vector3 o = face._n * -face._d; | 386 | Vector3 o = face._n * -face._d; |
387 | float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(), | 387 | float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(), |
388 | Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(), | 388 | Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(), |
389 | Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()}; | 389 | Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()}; |
390 | float sm = a[0] + a[1] + a[2]; | 390 | float sm = a[0] + a[1] + a[2]; |
391 | return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); | 391 | return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); |
392 | } | 392 | } |
393 | 393 | ||
394 | public Face FindBest() | 394 | public Face FindBest() |
395 | { | 395 | { |
396 | Face bf = null; | 396 | Face bf = null; |
397 | if (_root != null) | 397 | if (_root != null) |
398 | { | 398 | { |
399 | Face cf = _root; | 399 | Face cf = _root; |
400 | float bd = _infinity; | 400 | float bd = _infinity; |
401 | do | 401 | do |
402 | { | 402 | { |
403 | if (cf._d < bd) { bd = cf._d; bf = cf; } | 403 | if (cf._d < bd) { bd = cf._d; bf = cf; } |
404 | } while (null != (cf = cf._next)); | 404 | } while (null != (cf = cf._next)); |
405 | } | 405 | } |
406 | return bf; | 406 | return bf; |
407 | } | 407 | } |
408 | 408 | ||
409 | public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) | 409 | public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) |
410 | { | 410 | { |
411 | Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); | 411 | Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); |
412 | float len = nrm.Length(); | 412 | float len = nrm.Length(); |
413 | bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon && | 413 | bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon && |
414 | Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon && | 414 | Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon && |
415 | Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon); | 415 | Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon); |
416 | f._vertices[0] = a; | 416 | f._vertices[0] = a; |
417 | f._vertices[1] = b; | 417 | f._vertices[1] = b; |
418 | f._vertices[2] = c; | 418 | f._vertices[2] = c; |
419 | f._mark = 0; | 419 | f._mark = 0; |
420 | f._n = nrm / (len > 0 ? len : _infinity); | 420 | f._n = nrm / (len > 0 ? len : _infinity); |
421 | f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); | 421 | f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); |
422 | return valid; | 422 | return valid; |
423 | } | 423 | } |
424 | 424 | ||
425 | public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) | 425 | public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) |
426 | { | 426 | { |
427 | Face pf = new Face(); | 427 | Face pf = new Face(); |
428 | if (Set(ref pf, a, b, c)) | 428 | if (Set(ref pf, a, b, c)) |
429 | { | 429 | { |
430 | if (_root != null) _root._prev = pf; | 430 | if (_root != null) _root._prev = pf; |
431 | pf._prev = null; | 431 | pf._prev = null; |
432 | pf._next = _root; | 432 | pf._next = _root; |
433 | _root = pf; | 433 | _root = pf; |
434 | ++_nfaces; | 434 | ++_nfaces; |
435 | } | 435 | } |
436 | else | 436 | else |
437 | { | 437 | { |
438 | pf._prev = pf._next = null; | 438 | pf._prev = pf._next = null; |
439 | } | 439 | } |
440 | return (pf); | 440 | return (pf); |
441 | } | 441 | } |
442 | 442 | ||
443 | public void Detach(ref Face face) | 443 | public void Detach(ref Face face) |
444 | { | 444 | { |
445 | if (face._prev != null || face._next != null) | 445 | if (face._prev != null || face._next != null) |
446 | { | 446 | { |
447 | --_nfaces; | 447 | --_nfaces; |
448 | if (face == _root) | 448 | if (face == _root) |
449 | { | 449 | { |
450 | _root = face._next; | 450 | _root = face._next; |
451 | _root._prev = null; | 451 | _root._prev = null; |
452 | } | 452 | } |
453 | else | 453 | else |
454 | { | 454 | { |
455 | if (face._next == null) | 455 | if (face._next == null) |
456 | { | 456 | { |
457 | face._prev._next = null; | 457 | face._prev._next = null; |
458 | } | 458 | } |
459 | else | 459 | else |
460 | { | 460 | { |
461 | face._prev._next = face._next; | 461 | face._prev._next = face._next; |
462 | face._next._prev = face._prev; | 462 | face._next._prev = face._prev; |
463 | } | 463 | } |
464 | } | 464 | } |
465 | face._prev = face._next = null; | 465 | face._prev = face._next = null; |
466 | } | 466 | } |
467 | } | 467 | } |
468 | 468 | ||
469 | public void Link(ref Face f0, int e0, ref Face f1, int e1) | 469 | public void Link(ref Face f0, int e0, ref Face f1, int e1) |
470 | { | 470 | { |
471 | f0._faces[e0] = f1; f1._e[e1] = e0; | 471 | f0._faces[e0] = f1; f1._e[e1] = e0; |
472 | f1._faces[e1] = f0; f0._e[e0] = e1; | 472 | f1._faces[e1] = f0; f0._e[e0] = e1; |
473 | } | 473 | } |
474 | 474 | ||
475 | public Gjk.MinkowskiVertice Support(Vector3 w) | 475 | public Gjk.MinkowskiVertice Support(Vector3 w) |
476 | { | 476 | { |
477 | Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); | 477 | Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); |
478 | _gjk.Support(w, ref v); | 478 | _gjk.Support(w, ref v); |
479 | return v; | 479 | return v; |
480 | } | 480 | } |
481 | 481 | ||
482 | private static int[] mod3 ={ 0, 1, 2, 0, 1 }; | 482 | private static int[] mod3 ={ 0, 1, 2, 0, 1 }; |
483 | 483 | ||
484 | public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) | 484 | public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) |
485 | { | 485 | { |
486 | int ne = (0); | 486 | int ne = (0); |
487 | if (f._mark != markid) | 487 | if (f._mark != markid) |
488 | { | 488 | { |
489 | int e1 = (mod3[e + 1]); | 489 | int e1 = (mod3[e + 1]); |
490 | if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) | 490 | if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) |
491 | { | 491 | { |
492 | Face nf = NewFace(f._vertices[e1], f._vertices[e], w); | 492 | Face nf = NewFace(f._vertices[e1], f._vertices[e], w); |
493 | Link(ref nf, 0, ref f, e); | 493 | Link(ref nf, 0, ref f, e); |
494 | if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; | 494 | if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; |
495 | cf = nf; ne = 1; | 495 | cf = nf; ne = 1; |
496 | } | 496 | } |
497 | else | 497 | else |
498 | { | 498 | { |
499 | int e2 = (mod3[e + 2]); | 499 | int e2 = (mod3[e + 2]); |
500 | Detach(ref f); | 500 | Detach(ref f); |
501 | f._mark = markid; | 501 | f._mark = markid; |
502 | ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff); | 502 | ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff); |
503 | ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff); | 503 | ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff); |
504 | } | 504 | } |
505 | } | 505 | } |
506 | return (ne); | 506 | return (ne); |
507 | } | 507 | } |
508 | 508 | ||
509 | public float EvaluatePD() | 509 | public float EvaluatePD() |
510 | { | 510 | { |
511 | return EvaluatePD(_epaAccuracy); | 511 | return EvaluatePD(_epaAccuracy); |
512 | } | 512 | } |
513 | 513 | ||
514 | private int[,] fidx; | 514 | private int[,] fidx; |
515 | private int[,] eidx; | 515 | private int[,] eidx; |
516 | 516 | ||
517 | public float EvaluatePD(float accuracy) | 517 | public float EvaluatePD(float accuracy) |
518 | { | 518 | { |
519 | //Block* sablock = sa->BeginBlock(); | 519 | //Block* sablock = sa->BeginBlock(); |
520 | Face bestface = null; | 520 | Face bestface = null; |
521 | int markid = 1; | 521 | int markid = 1; |
522 | _depth = -_infinity; | 522 | _depth = -_infinity; |
523 | _normal = new Vector3(); | 523 | _normal = new Vector3(); |
524 | _root = null; | 524 | _root = null; |
525 | _nfaces = 0; | 525 | _nfaces = 0; |
526 | _iterations = 0; | 526 | _iterations = 0; |
527 | _failed = false; | 527 | _failed = false; |
528 | /* Prepare hull */ | 528 | /* Prepare hull */ |
529 | if (_gjk.EncloseOrigin()) | 529 | if (_gjk.EncloseOrigin()) |
530 | { | 530 | { |
531 | int nfidx = 0; | 531 | int nfidx = 0; |
532 | int neidx = 0; | 532 | int neidx = 0; |
533 | Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; | 533 | Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; |
534 | Face[] basefaces = new Face[6]; | 534 | Face[] basefaces = new Face[6]; |
535 | switch (_gjk.Order) | 535 | switch (_gjk.Order) |
536 | { | 536 | { |
537 | /* Tetrahedron */ | 537 | /* Tetrahedron */ |
538 | case 3: | 538 | case 3: |
539 | { | 539 | { |
540 | fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } }; | 540 | fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } }; |
541 | eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } }; | 541 | eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } }; |
542 | nfidx = 4; neidx = 6; | 542 | nfidx = 4; neidx = 6; |
543 | } break; | 543 | } break; |
544 | /* Hexahedron */ | 544 | /* Hexahedron */ |
545 | case 4: | 545 | case 4: |
546 | { | 546 | { |
547 | fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } }; | 547 | fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } }; |
548 | eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } }; | 548 | eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } }; |
549 | nfidx = 6; neidx = 9; | 549 | nfidx = 6; neidx = 9; |
550 | } break; | 550 | } break; |
551 | } | 551 | } |
552 | int i; | 552 | int i; |
553 | 553 | ||
554 | for (i = 0; i <= _gjk.Order; ++i) | 554 | for (i = 0; i <= _gjk.Order; ++i) |
555 | { | 555 | { |
556 | //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); | 556 | //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); |
557 | basemkv[i] = new Gjk.MinkowskiVertice(); | 557 | basemkv[i] = new Gjk.MinkowskiVertice(); |
558 | basemkv[i].Vertice = _gjk.Simplex[i].Vertice; | 558 | basemkv[i].Vertice = _gjk.Simplex[i].Vertice; |
559 | basemkv[i].Ray = _gjk.Simplex[i].Ray; | 559 | basemkv[i].Ray = _gjk.Simplex[i].Ray; |
560 | } | 560 | } |
561 | for (i = 0; i < nfidx; ++i) | 561 | for (i = 0; i < nfidx; ++i) |
562 | { | 562 | { |
563 | basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); | 563 | basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); |
564 | } | 564 | } |
565 | for (i = 0; i < neidx; ++i) | 565 | for (i = 0; i < neidx; ++i) |
566 | { | 566 | { |
567 | Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); | 567 | Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); |
568 | } | 568 | } |
569 | } | 569 | } |
570 | if (0 == _nfaces) | 570 | if (0 == _nfaces) |
571 | { | 571 | { |
572 | return _depth; | 572 | return _depth; |
573 | } | 573 | } |
574 | /* Expand hull */ | 574 | /* Expand hull */ |
575 | for (; _iterations < _epaMaxIterations; ++_iterations) | 575 | for (; _iterations < _epaMaxIterations; ++_iterations) |
576 | { | 576 | { |
577 | Face bf = FindBest(); | 577 | Face bf = FindBest(); |
578 | if (bf != null) | 578 | if (bf != null) |
579 | { | 579 | { |
580 | Gjk.MinkowskiVertice w = Support(-bf._n); | 580 | Gjk.MinkowskiVertice w = Support(-bf._n); |
581 | float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; | 581 | float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; |
582 | bestface = bf; | 582 | bestface = bf; |
583 | if (d < -accuracy) | 583 | if (d < -accuracy) |
584 | { | 584 | { |
585 | Face cf = null; | 585 | Face cf = null; |
586 | Face ff = null; | 586 | Face ff = null; |
587 | int nf = 0; | 587 | int nf = 0; |
588 | Detach(ref bf); | 588 | Detach(ref bf); |
589 | bf._mark = ++markid; | 589 | bf._mark = ++markid; |
590 | for (int i = 0; i < 3; ++i) | 590 | for (int i = 0; i < 3; ++i) |
591 | { | 591 | { |
592 | nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); | 592 | nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); |
593 | } | 593 | } |
594 | if (nf <= 2) { break; } | 594 | if (nf <= 2) { break; } |
595 | Link(ref cf, 1, ref ff, 2); | 595 | Link(ref cf, 1, ref ff, 2); |
596 | } | 596 | } |
597 | else break; | 597 | else break; |
598 | } | 598 | } |
599 | else break; | 599 | else break; |
600 | } | 600 | } |
601 | /* Extract contact */ | 601 | /* Extract contact */ |
602 | if (bestface != null) | 602 | if (bestface != null) |
603 | { | 603 | { |
604 | Vector3 b = GetCoordinates(bestface); | 604 | Vector3 b = GetCoordinates(bestface); |
605 | _normal = bestface._n; | 605 | _normal = bestface._n; |
606 | _depth = Max(0, bestface._d); | 606 | _depth = Max(0, bestface._d); |
607 | for (int i = 0; i < 2; ++i) | 607 | for (int i = 0; i < 2; ++i) |
608 | { | 608 | { |
609 | float s = i != 0 ? -1 : 1; | 609 | float s = i != 0 ? -1 : 1; |
610 | for (int j = 0; j < 3; ++j) | 610 | for (int j = 0; j < 3; ++j) |
611 | { | 611 | { |
612 | _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); | 612 | _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); |
613 | } | 613 | } |
614 | } | 614 | } |
615 | _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z; | 615 | _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z; |
616 | _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z; | 616 | _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z; |
617 | } | 617 | } |
618 | else _failed = true; | 618 | else _failed = true; |
619 | return _depth; | 619 | return _depth; |
620 | } | 620 | } |
621 | 621 | ||
622 | private float Max(float a, float b) | 622 | private float Max(float a, float b) |
623 | { | 623 | { |
624 | return (a > b ? a : b); | 624 | return (a > b ? a : b); |
625 | } | 625 | } |
626 | 626 | ||
627 | private float Min(float a, float b) | 627 | private float Min(float a, float b) |
628 | { | 628 | { |
629 | return (a < b ? a : b); | 629 | return (a < b ? a : b); |
630 | } | 630 | } |
631 | } | 631 | } |
632 | } | 632 | } |
633 | } | 633 | } |