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