aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs540
1 files changed, 270 insertions, 270 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
index 8933638..2b4e801 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
@@ -1,270 +1,270 @@
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 /// SphereBoxCollisionAlgorithm provides sphere-box collision detection. 30 /// SphereBoxCollisionAlgorithm provides sphere-box collision detection.
31 /// Other features are frame-coherency (persistent data) and collision response. 31 /// Other features are frame-coherency (persistent data) and collision response.
32 /// </summary> 32 /// </summary>
33 public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable 33 public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable
34 { 34 {
35 private bool _ownManifold; 35 private bool _ownManifold;
36 private PersistentManifold _manifold; 36 private PersistentManifold _manifold;
37 private bool _isSwapped; 37 private bool _isSwapped;
38 38
39 public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped) 39 public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped)
40 : base(collisionAlgorithmConstructionInfo) 40 : base(collisionAlgorithmConstructionInfo)
41 { 41 {
42 _ownManifold = false; 42 _ownManifold = false;
43 _manifold = manifold; 43 _manifold = manifold;
44 _isSwapped = isSwapped; 44 _isSwapped = isSwapped;
45 45
46 CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA; 46 CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA;
47 CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB; 47 CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB;
48 48
49 if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject)) 49 if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject))
50 { 50 {
51 _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject); 51 _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject);
52 _ownManifold = true; 52 _ownManifold = true;
53 } 53 }
54 } 54 }
55 55
56 public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius) 56 public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius)
57 { 57 {
58 pointOnBox = new Vector3(); 58 pointOnBox = new Vector3();
59 pointOnSphere = new Vector3(); 59 pointOnSphere = new Vector3();
60 60
61 float margins; 61 float margins;
62 Vector3[] bounds = new Vector3[2]; 62 Vector3[] bounds = new Vector3[2];
63 BoxShape boxShape = boxObject.CollisionShape as BoxShape; 63 BoxShape boxShape = boxObject.CollisionShape as BoxShape;
64 64
65 bounds[0] = -boxShape.HalfExtents; 65 bounds[0] = -boxShape.HalfExtents;
66 bounds[1] = boxShape.HalfExtents; 66 bounds[1] = boxShape.HalfExtents;
67 67
68 margins = boxShape.Margin; //also add sphereShape margin? 68 margins = boxShape.Margin; //also add sphereShape margin?
69 69
70 Matrix m44T = boxObject.WorldTransform; 70 Matrix m44T = boxObject.WorldTransform;
71 71
72 Vector3[] boundsVec = new Vector3[2]; 72 Vector3[] boundsVec = new Vector3[2];
73 float penetration; 73 float penetration;
74 74
75 boundsVec[0] = bounds[0]; 75 boundsVec[0] = bounds[0];
76 boundsVec[1] = bounds[1]; 76 boundsVec[1] = bounds[1];
77 77
78 Vector3 marginsVec = new Vector3(margins, margins, margins); 78 Vector3 marginsVec = new Vector3(margins, margins, margins);
79 79
80 // add margins 80 // add margins
81 bounds[0] += marginsVec; 81 bounds[0] += marginsVec;
82 bounds[1] -= marginsVec; 82 bounds[1] -= marginsVec;
83 83
84 ///////////////////////////////////////////////// 84 /////////////////////////////////////////////////
85 85
86 Vector3 tmp, prel, normal, v3P; 86 Vector3 tmp, prel, normal, v3P;
87 Vector3[] n = new Vector3[6]; 87 Vector3[] n = new Vector3[6];
88 float sep = 10000000.0f, sepThis; 88 float sep = 10000000.0f, sepThis;
89 89
90 n[0] = new Vector3(-1.0f, 0.0f, 0.0f); 90 n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
91 n[1] = new Vector3(0.0f, -1.0f, 0.0f); 91 n[1] = new Vector3(0.0f, -1.0f, 0.0f);
92 n[2] = new Vector3(0.0f, 0.0f, -1.0f); 92 n[2] = new Vector3(0.0f, 0.0f, -1.0f);
93 n[3] = new Vector3(1.0f, 0.0f, 0.0f); 93 n[3] = new Vector3(1.0f, 0.0f, 0.0f);
94 n[4] = new Vector3(0.0f, 1.0f, 0.0f); 94 n[4] = new Vector3(0.0f, 1.0f, 0.0f);
95 n[5] = new Vector3(0.0f, 0.0f, 1.0f); 95 n[5] = new Vector3(0.0f, 0.0f, 1.0f);
96 96
97 // convert point in local space 97 // convert point in local space
98 prel = MathHelper.InvXForm(m44T, sphereCenter); 98 prel = MathHelper.InvXForm(m44T, sphereCenter);
99 99
100 bool found = false; 100 bool found = false;
101 101
102 v3P = prel; 102 v3P = prel;
103 103
104 for (int i = 0; i < 6; i++) 104 for (int i = 0; i < 6; i++)
105 { 105 {
106 int j = i < 3 ? 0 : 1; 106 int j = i < 3 ? 0 : 1;
107 if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f) 107 if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f)
108 { 108 {
109 v3P = v3P - n[i] * sepThis; 109 v3P = v3P - n[i] * sepThis;
110 found = true; 110 found = true;
111 } 111 }
112 } 112 }
113 113
114 // 114 //
115 115
116 if (found) 116 if (found)
117 { 117 {
118 bounds[0] = boundsVec[0]; 118 bounds[0] = boundsVec[0];
119 bounds[1] = boundsVec[1]; 119 bounds[1] = boundsVec[1];
120 120
121 normal = Vector3.Normalize(prel - v3P); 121 normal = Vector3.Normalize(prel - v3P);
122 pointOnBox = v3P + normal * margins; 122 pointOnBox = v3P + normal * margins;
123 pointOnSphere = prel - normal * radius; 123 pointOnSphere = prel - normal * radius;
124 124
125 if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f) 125 if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f)
126 { 126 {
127 return 1.0f; 127 return 1.0f;
128 } 128 }
129 129
130 // transform back in world space 130 // transform back in world space
131 tmp = MathHelper.MatrixToVector(m44T, pointOnBox); 131 tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
132 pointOnBox = tmp; 132 pointOnBox = tmp;
133 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); 133 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
134 pointOnSphere = tmp; 134 pointOnSphere = tmp;
135 float seps2 = (pointOnBox - pointOnSphere).LengthSquared(); 135 float seps2 = (pointOnBox - pointOnSphere).LengthSquared();
136 136
137 //if this fails, fallback into deeper penetration case, below 137 //if this fails, fallback into deeper penetration case, below
138 if (seps2 > MathHelper.Epsilon) 138 if (seps2 > MathHelper.Epsilon)
139 { 139 {
140 sep = -(float)Math.Sqrt(seps2); 140 sep = -(float)Math.Sqrt(seps2);
141 normal = (pointOnBox - pointOnSphere); 141 normal = (pointOnBox - pointOnSphere);
142 normal *= 1f / sep; 142 normal *= 1f / sep;
143 } 143 }
144 return sep; 144 return sep;
145 } 145 }
146 146
147 ////////////////////////////////////////////////// 147 //////////////////////////////////////////////////
148 // Deep penetration case 148 // Deep penetration case
149 149
150 penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]); 150 penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]);
151 151
152 bounds[0] = boundsVec[0]; 152 bounds[0] = boundsVec[0];
153 bounds[1] = boundsVec[1]; 153 bounds[1] = boundsVec[1];
154 154
155 if (penetration <= 0.0f) 155 if (penetration <= 0.0f)
156 return (penetration - margins); 156 return (penetration - margins);
157 else 157 else
158 return 1.0f; 158 return 1.0f;
159 } 159 }
160 160
161 public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax) 161 public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax)
162 { 162 {
163 Vector3[] bounds = new Vector3[2]; 163 Vector3[] bounds = new Vector3[2];
164 164
165 bounds[0] = aabbMin; 165 bounds[0] = aabbMin;
166 bounds[1] = aabbMax; 166 bounds[1] = aabbMax;
167 167
168 Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3(); 168 Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3();
169 Vector3[] n = new Vector3[6]; 169 Vector3[] n = new Vector3[6];
170 float sep = -10000000.0f, sepThis; 170 float sep = -10000000.0f, sepThis;
171 171
172 n[0] = new Vector3(-1.0f, 0.0f, 0.0f); 172 n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
173 n[1] = new Vector3(0.0f, -1.0f, 0.0f); 173 n[1] = new Vector3(0.0f, -1.0f, 0.0f);
174 n[2] = new Vector3(0.0f, 0.0f, -1.0f); 174 n[2] = new Vector3(0.0f, 0.0f, -1.0f);
175 n[3] = new Vector3(1.0f, 0.0f, 0.0f); 175 n[3] = new Vector3(1.0f, 0.0f, 0.0f);
176 n[4] = new Vector3(0.0f, 1.0f, 0.0f); 176 n[4] = new Vector3(0.0f, 1.0f, 0.0f);
177 n[5] = new Vector3(0.0f, 0.0f, 1.0f); 177 n[5] = new Vector3(0.0f, 0.0f, 1.0f);
178 178
179 Matrix m44T = boxObject.WorldTransform; 179 Matrix m44T = boxObject.WorldTransform;
180 180
181 // convert point in local space 181 // convert point in local space
182 prel = MathHelper.InvXForm(m44T, sphereCenter); 182 prel = MathHelper.InvXForm(m44T, sphereCenter);
183 183
184 /////////// 184 ///////////
185 185
186 for (int i = 0; i < 6; i++) 186 for (int i = 0; i < 6; i++)
187 { 187 {
188 int j = i < 3 ? 0 : 1; 188 int j = i < 3 ? 0 : 1;
189 if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f; 189 if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f;
190 if (sepThis > sep) 190 if (sepThis > sep)
191 { 191 {
192 p0 = bounds[j]; 192 p0 = bounds[j];
193 normal = n[i]; 193 normal = n[i];
194 sep = sepThis; 194 sep = sepThis;
195 } 195 }
196 } 196 }
197 197
198 pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0))); 198 pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0)));
199 pointOnSphere = pointOnBox + normal * sep; 199 pointOnSphere = pointOnBox + normal * sep;
200 200
201 // transform back in world space 201 // transform back in world space
202 tmp = MathHelper.MatrixToVector(m44T, pointOnBox); 202 tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
203 pointOnBox = tmp; 203 pointOnBox = tmp;
204 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); 204 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
205 pointOnSphere = tmp; 205 pointOnSphere = tmp;
206 normal = Vector3.Normalize(pointOnBox - pointOnSphere); 206 normal = Vector3.Normalize(pointOnBox - pointOnSphere);
207 207
208 return sep; 208 return sep;
209 } 209 }
210 210
211 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 211 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
212 { 212 {
213 if (_manifold == null) 213 if (_manifold == null)
214 return; 214 return;
215 215
216 CollisionObject sphereObject = _isSwapped ? bodyB : bodyA; 216 CollisionObject sphereObject = _isSwapped ? bodyB : bodyA;
217 CollisionObject boxObject = _isSwapped ? bodyA : bodyB; 217 CollisionObject boxObject = _isSwapped ? bodyA : bodyB;
218 218
219 SphereShape sphereA = sphereObject.CollisionShape as SphereShape; 219 SphereShape sphereA = sphereObject.CollisionShape as SphereShape;
220 220
221 Vector3 pOnBox, pOnSphere; 221 Vector3 pOnBox, pOnSphere;
222 Vector3 sphereCenter = sphereObject.WorldTransform.Translation; 222 Vector3 sphereCenter = sphereObject.WorldTransform.Translation;
223 float radius = sphereA.Radius; 223 float radius = sphereA.Radius;
224 224
225 float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius); 225 float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius);
226 226
227 if (dist < MathHelper.Epsilon) 227 if (dist < MathHelper.Epsilon)
228 { 228 {
229 Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere); 229 Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere);
230 230
231 // report a contact. internally this will be kept persistent, and contact reduction is done 231 // report a contact. internally this will be kept persistent, and contact reduction is done
232 resultOut.SetPersistentManifold(_manifold); 232 resultOut.SetPersistentManifold(_manifold);
233 resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist); 233 resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist);
234 } 234 }
235 } 235 }
236 236
237 public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 237 public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
238 { 238 {
239 //not yet 239 //not yet
240 return 1; 240 return 1;
241 } 241 }
242 242
243 public class CreateFunc : CollisionAlgorithmCreateFunction 243 public class CreateFunc : CollisionAlgorithmCreateFunction
244 { 244 {
245 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 245 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
246 { 246 {
247 if (!IsSwapped) 247 if (!IsSwapped)
248 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false); 248 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
249 else 249 else
250 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true); 250 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
251 } 251 }
252 } 252 }
253 253
254 #region IDisposable Members 254 #region IDisposable Members
255 public void Dispose() 255 public void Dispose()
256 { 256 {
257 Dispose(true); 257 Dispose(true);
258 } 258 }
259 259
260 public void Dispose(bool disposing) 260 public void Dispose(bool disposing)
261 { 261 {
262 if (disposing && _ownManifold) 262 if (disposing && _ownManifold)
263 { 263 {
264 if (_manifold != null) 264 if (_manifold != null)
265 Dispatcher.ReleaseManifold(_manifold); 265 Dispatcher.ReleaseManifold(_manifold);
266 } 266 }
267 } 267 }
268 #endregion 268 #endregion
269 } 269 }
270} 270}
generated by cgit v1.1 at 2024-09-12 22:15:16 +0000