aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs1266
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
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace 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}
generated by cgit v1.1 at 2024-12-29 11:34:39 +0000