diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs | 304 |
1 files changed, 304 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs new file mode 100644 index 0000000..957843b --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs | |||
@@ -0,0 +1,304 @@ | |||
1 | /* | ||
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 | ||
4 | |||
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 | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
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 | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SimulationIslandManager | ||
30 | { | ||
31 | private UnionFind _unionFind = new UnionFind(); | ||
32 | |||
33 | public void InitUnionFind(int n) | ||
34 | { | ||
35 | _unionFind.Reset(n); | ||
36 | } | ||
37 | |||
38 | public UnionFind UnionFind { get { return _unionFind; } } | ||
39 | |||
40 | public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher) | ||
41 | { | ||
42 | InitUnionFind(world.CollisionObjectsCount); | ||
43 | |||
44 | // put the index into m_controllers into m_tag | ||
45 | int index = 0; | ||
46 | for (int i = 0; i < world.CollisionObjects.Count; i++) | ||
47 | { | ||
48 | world.CollisionObjects[i].IslandTag = index; | ||
49 | world.CollisionObjects[i].HitFraction = 1; | ||
50 | world.CollisionObjects[i].CompanionID = -1; | ||
51 | index++; | ||
52 | } | ||
53 | // do the union find | ||
54 | FindUnions(dispatcher); | ||
55 | } | ||
56 | |||
57 | public virtual void StoreIslandActivationState(CollisionWorld world) | ||
58 | { | ||
59 | // put the islandId ('find' value) into m_tag | ||
60 | int index = 0; | ||
61 | for (int i = 0; i < world.CollisionObjects.Count; i++) | ||
62 | { | ||
63 | if (world.CollisionObjects[i].MergesSimulationIslands) | ||
64 | { | ||
65 | world.CollisionObjects[i].IslandTag = _unionFind.Find(index); | ||
66 | world.CollisionObjects[i].CompanionID = -1; | ||
67 | } | ||
68 | else | ||
69 | { | ||
70 | world.CollisionObjects[i].IslandTag = -1; | ||
71 | world.CollisionObjects[i].CompanionID = -2; | ||
72 | } | ||
73 | index++; | ||
74 | } | ||
75 | } | ||
76 | |||
77 | public void FindUnions(IDispatcher dispatcher) | ||
78 | { | ||
79 | for (int i = 0; i < dispatcher.ManifoldCount; i++) | ||
80 | { | ||
81 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); | ||
82 | //static objects (invmass 0.f) don't merge ! | ||
83 | |||
84 | CollisionObject colObjA = manifold.BodyA as CollisionObject; | ||
85 | CollisionObject colObjB = manifold.BodyB as CollisionObject; | ||
86 | |||
87 | if (((colObjA != null) && (colObjA.MergesSimulationIslands)) && | ||
88 | ((colObjB != null) && (colObjB.MergesSimulationIslands))) | ||
89 | { | ||
90 | _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag); | ||
91 | } | ||
92 | } | ||
93 | } | ||
94 | |||
95 | public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback) | ||
96 | { | ||
97 | //we are going to sort the unionfind array, and store the element id in the size | ||
98 | //afterwards, we clean unionfind, to make sure no-one uses it anymore | ||
99 | UnionFind.SortIslands(); | ||
100 | int numElem = UnionFind.ElementCount; | ||
101 | |||
102 | int endIslandIndex = 1; | ||
103 | int startIslandIndex; | ||
104 | |||
105 | //update the sleeping state for bodies, if all are sleeping | ||
106 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) | ||
107 | { | ||
108 | int islandId = UnionFind[startIslandIndex].ID; | ||
109 | for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) | ||
110 | { | ||
111 | } | ||
112 | |||
113 | //int numSleeping = 0; | ||
114 | |||
115 | bool allSleeping = true; | ||
116 | |||
117 | int idx; | ||
118 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
119 | { | ||
120 | int i = UnionFind[idx].Size; | ||
121 | |||
122 | CollisionObject colObjA = collisionObjects[i]; | ||
123 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
124 | { | ||
125 | Console.WriteLine("error in island management"); | ||
126 | } | ||
127 | |||
128 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
129 | if (colObjA.IslandTag == islandId) | ||
130 | { | ||
131 | if (colObjA.ActivationState == ActivationState.Active) | ||
132 | { | ||
133 | allSleeping = false; | ||
134 | } | ||
135 | if (colObjA.ActivationState == ActivationState.DisableDeactivation) | ||
136 | { | ||
137 | allSleeping = false; | ||
138 | } | ||
139 | } | ||
140 | } | ||
141 | |||
142 | |||
143 | if (allSleeping) | ||
144 | { | ||
145 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
146 | { | ||
147 | int i = UnionFind[idx].Size; | ||
148 | CollisionObject colObjA = collisionObjects[i]; | ||
149 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
150 | { | ||
151 | Console.WriteLine("error in island management"); | ||
152 | } | ||
153 | |||
154 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
155 | |||
156 | if (colObjA.IslandTag == islandId) | ||
157 | { | ||
158 | colObjA.ActivationState =ActivationState.IslandSleeping; | ||
159 | } | ||
160 | } | ||
161 | } | ||
162 | else | ||
163 | { | ||
164 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
165 | { | ||
166 | int i = UnionFind[idx].Size; | ||
167 | |||
168 | CollisionObject colObjA = collisionObjects[i]; | ||
169 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
170 | { | ||
171 | Console.WriteLine("error in island management"); | ||
172 | } | ||
173 | |||
174 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
175 | |||
176 | if (colObjA.IslandTag == islandId) | ||
177 | { | ||
178 | if (colObjA.ActivationState == ActivationState.IslandSleeping) | ||
179 | { | ||
180 | colObjA.ActivationState = ActivationState.WantsDeactivation; | ||
181 | } | ||
182 | } | ||
183 | } | ||
184 | } | ||
185 | } | ||
186 | |||
187 | //int maxNumManifolds = dispatcher.ManifoldCount; | ||
188 | List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount); | ||
189 | |||
190 | for (int i = 0; i < dispatcher.ManifoldCount; i++) | ||
191 | { | ||
192 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); | ||
193 | |||
194 | CollisionObject colObjA = manifold.BodyA as CollisionObject; | ||
195 | CollisionObject colObjB = manifold.BodyB as CollisionObject; | ||
196 | |||
197 | //todo: check sleeping conditions! | ||
198 | if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) || | ||
199 | ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping)) | ||
200 | { | ||
201 | |||
202 | //kinematic objects don't merge islands, but wake up all connected objects | ||
203 | if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping) | ||
204 | { | ||
205 | colObjB.Activate(); | ||
206 | } | ||
207 | if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping) | ||
208 | { | ||
209 | colObjA.Activate(); | ||
210 | } | ||
211 | |||
212 | //filtering for response | ||
213 | if (dispatcher.NeedsResponse(colObjA, colObjB)) | ||
214 | islandmanifold.Add(manifold); | ||
215 | } | ||
216 | } | ||
217 | |||
218 | int numManifolds = islandmanifold.Count; | ||
219 | |||
220 | // Sort manifolds, based on islands | ||
221 | // Sort the vector using predicate and std::sort | ||
222 | islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate)); | ||
223 | |||
224 | //now process all active islands (sets of manifolds for now) | ||
225 | int startManifoldIndex = 0; | ||
226 | int endManifoldIndex = 1; | ||
227 | |||
228 | List<CollisionObject> islandBodies = new List<CollisionObject>(); | ||
229 | |||
230 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) | ||
231 | { | ||
232 | int islandId = UnionFind[startIslandIndex].ID; | ||
233 | bool islandSleeping = false; | ||
234 | for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) | ||
235 | { | ||
236 | int i = UnionFind[endIslandIndex].Size; | ||
237 | CollisionObject colObjA = collisionObjects[i]; | ||
238 | islandBodies.Add(colObjA); | ||
239 | if (!colObjA.IsActive) | ||
240 | islandSleeping = true; | ||
241 | } | ||
242 | |||
243 | //find the accompanying contact manifold for this islandId | ||
244 | int numIslandManifolds = 0; | ||
245 | List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds); | ||
246 | |||
247 | if (startManifoldIndex < numManifolds) | ||
248 | { | ||
249 | int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]); | ||
250 | if (curIslandID == islandId) | ||
251 | { | ||
252 | for (int k = startManifoldIndex; k < islandmanifold.Count; k++) | ||
253 | { | ||
254 | startManifold.Add(islandmanifold[k]); | ||
255 | } | ||
256 | for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { } | ||
257 | |||
258 | // Process the actual simulation, only if not sleeping/deactivated | ||
259 | numIslandManifolds = endManifoldIndex - startManifoldIndex; | ||
260 | } | ||
261 | } | ||
262 | |||
263 | if (!islandSleeping) | ||
264 | { | ||
265 | callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId); | ||
266 | } | ||
267 | |||
268 | if (numIslandManifolds != 0) | ||
269 | { | ||
270 | startManifoldIndex = endManifoldIndex; | ||
271 | } | ||
272 | |||
273 | islandBodies.Clear(); | ||
274 | } | ||
275 | } | ||
276 | |||
277 | private static int GetIslandId(PersistentManifold lhs) | ||
278 | { | ||
279 | int islandId; | ||
280 | CollisionObject rcolObjA = lhs.BodyA as CollisionObject; | ||
281 | CollisionObject rcolObjB = lhs.BodyB as CollisionObject; | ||
282 | islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag; | ||
283 | return islandId; | ||
284 | } | ||
285 | |||
286 | private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs) | ||
287 | { | ||
288 | int rIslandIdA, lIslandIdB; | ||
289 | rIslandIdA = GetIslandId(rhs); | ||
290 | lIslandIdB = GetIslandId(lhs); | ||
291 | //return lIslandId0 < rIslandId0; | ||
292 | if (lIslandIdB < rIslandIdA) | ||
293 | return -1; | ||
294 | //else if (lIslandIdB > rIslandIdA) | ||
295 | // return 1; | ||
296 | return 1; | ||
297 | } | ||
298 | |||
299 | public interface IIslandCallback | ||
300 | { | ||
301 | void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID); | ||
302 | } | ||
303 | } | ||
304 | } | ||