1 | #region MIT License |
---|
2 | /* |
---|
3 | * Copyright (c) 2005-2008 Jonathan Mark Porter. http://physics2d.googlepages.com/ |
---|
4 | * |
---|
5 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
---|
6 | * of this software and associated documentation files (the "Software"), to deal |
---|
7 | * in the Software without restriction, including without limitation the rights to |
---|
8 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of |
---|
9 | * the Software, and to permit persons to whom the Software is furnished to do so, |
---|
10 | * subject to the following conditions: |
---|
11 | * |
---|
12 | * The above copyright notice and this permission notice shall be |
---|
13 | * included in all copies or substantial portions of the Software. |
---|
14 | * |
---|
15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, |
---|
16 | * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR |
---|
17 | * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE |
---|
18 | * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, |
---|
19 | * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
---|
20 | * OTHER DEALINGS IN THE SOFTWARE. |
---|
21 | */ |
---|
22 | #endregion |
---|
23 | |
---|
24 | |
---|
25 | |
---|
26 | |
---|
27 | #if UseDouble |
---|
28 | using Scalar = System.Double; |
---|
29 | #else |
---|
30 | using Scalar = System.Single; |
---|
31 | #endif |
---|
32 | using System; |
---|
33 | using System.Collections.ObjectModel; |
---|
34 | using System.Collections.Generic; |
---|
35 | |
---|
36 | using AdvanceMath; |
---|
37 | |
---|
38 | using Physics2DDotNet.Solvers; |
---|
39 | using Physics2DDotNet.Detectors; |
---|
40 | using Physics2DDotNet.Collections; |
---|
41 | using Physics2DDotNet.Shapes; |
---|
42 | using Physics2DDotNet.PhysicsLogics; |
---|
43 | using Physics2DDotNet.Joints; |
---|
44 | |
---|
45 | namespace Physics2DDotNet |
---|
46 | { |
---|
47 | /// <summary> |
---|
48 | /// The Engine that will Apply Physics to object added to it. |
---|
49 | /// </summary> |
---|
50 | #if !(WINDOWS_PHONE || XBOX) |
---|
51 | [Serializable] |
---|
52 | #endif |
---|
53 | public sealed class PhysicsEngine |
---|
54 | { |
---|
55 | public class LogicComparer : IComparer<PhysicsLogic> |
---|
56 | { |
---|
57 | public int Compare(PhysicsLogic x, PhysicsLogic y) |
---|
58 | { |
---|
59 | return x.Order.CompareTo(y.Order); |
---|
60 | } |
---|
61 | } |
---|
62 | |
---|
63 | #region static/const fields |
---|
64 | /// <summary> |
---|
65 | /// This is the ID the first body added to the engine will get. |
---|
66 | /// </summary> |
---|
67 | const int firstBodyID = 1; |
---|
68 | static LogicComparer logicComparer = new LogicComparer(); |
---|
69 | #endregion |
---|
70 | #region static methods |
---|
71 | private static void PreCheckItem(Joint item) |
---|
72 | { |
---|
73 | if (item == null) { throw new ArgumentNullException("item"); } |
---|
74 | item.isChecked = false; |
---|
75 | } |
---|
76 | private static void PreCheckItem(Body item) |
---|
77 | { |
---|
78 | if (item == null) { throw new ArgumentNullException("item"); } |
---|
79 | item.isChecked = false; |
---|
80 | } |
---|
81 | private static void PreCheckItem(PhysicsLogic item) |
---|
82 | { |
---|
83 | if (item == null) { throw new ArgumentNullException("item"); } |
---|
84 | item.isChecked = false; |
---|
85 | } |
---|
86 | private static void CheckItem(Joint item) |
---|
87 | { |
---|
88 | if (item.Engine != null || item.isChecked) { throw new InvalidOperationException("A Joint cannot be added to more then one engine or added twice."); } |
---|
89 | item.isChecked = true; |
---|
90 | } |
---|
91 | private static void CheckItem(Body item) |
---|
92 | { |
---|
93 | if (item.Engine != null || item.isChecked) { throw new InvalidOperationException("A Body cannot be added to more then one engine or added twice."); } |
---|
94 | item.isChecked = true; |
---|
95 | } |
---|
96 | private static void CheckItem(PhysicsLogic item) |
---|
97 | { |
---|
98 | if (item.Engine != null || item.isChecked) { throw new InvalidOperationException("A PhysicsLogic cannot be added to more then one engine or added twice."); } |
---|
99 | item.isChecked = true; |
---|
100 | } |
---|
101 | #endregion |
---|
102 | #region events |
---|
103 | /// <summary> |
---|
104 | /// Generated when Bodies are truly added to the Engine. |
---|
105 | /// </summary> |
---|
106 | public event EventHandler<CollectionEventArgs<Body>> BodiesAdded; |
---|
107 | /// <summary> |
---|
108 | /// Generated when Joints are truly added to the Engine. |
---|
109 | /// </summary> |
---|
110 | public event EventHandler<CollectionEventArgs<Joint>> JointsAdded; |
---|
111 | /// <summary> |
---|
112 | /// Generated when PhysicsLogics are truly added to the Engine. |
---|
113 | /// </summary> |
---|
114 | public event EventHandler<CollectionEventArgs<PhysicsLogic>> LogicsAdded; |
---|
115 | /// <summary> |
---|
116 | /// Generated when a Bodies are removed to the Engine. |
---|
117 | /// </summary> |
---|
118 | public event EventHandler<CollectionEventArgs<Body>> BodiesRemoved; |
---|
119 | /// <summary> |
---|
120 | /// Generated when a Joints are removed to the Engine. |
---|
121 | /// </summary> |
---|
122 | public event EventHandler<CollectionEventArgs<Joint>> JointsRemoved; |
---|
123 | /// <summary> |
---|
124 | /// Generated when a PhysicsLogics are removed to the Engine. |
---|
125 | /// </summary> |
---|
126 | public event EventHandler<CollectionEventArgs<PhysicsLogic>> LogicsRemoved; |
---|
127 | /// <summary> |
---|
128 | /// Generated when the engine is update; |
---|
129 | /// </summary> |
---|
130 | public event EventHandler<UpdatedEventArgs> Updated; |
---|
131 | #endregion |
---|
132 | #region fields |
---|
133 | private int updateCount; |
---|
134 | private int nextBodyID; |
---|
135 | #if !WINDOWS_PHONE |
---|
136 | [NonSerialized] |
---|
137 | #endif |
---|
138 | object syncRoot; |
---|
139 | #if !WINDOWS_PHONE |
---|
140 | [NonSerialized] |
---|
141 | #endif |
---|
142 | AdvReaderWriterLock rwLock; |
---|
143 | #if !WINDOWS_PHONE |
---|
144 | [NonSerialized] |
---|
145 | #endif |
---|
146 | internal bool inUpdate; |
---|
147 | internal bool logicsNeedSorting; |
---|
148 | |
---|
149 | private List<PhysicsLogic> logics; |
---|
150 | internal List<Body> bodies; |
---|
151 | internal List<Joint> joints; |
---|
152 | |
---|
153 | private List<PhysicsLogic> pendingLogics; |
---|
154 | private List<Joint> pendingJoints; |
---|
155 | private List<Body> pendingBodies; |
---|
156 | private List<BodyProxy> pendingProxies; |
---|
157 | |
---|
158 | private List<PhysicsLogic> removedLogics; |
---|
159 | private List<Joint> removedJoints; |
---|
160 | private List<Body> removedBodies; |
---|
161 | |
---|
162 | private CollisionSolver solver; |
---|
163 | private BroadPhaseCollisionDetector broadPhase; |
---|
164 | |
---|
165 | #endregion |
---|
166 | #region constructors |
---|
167 | public PhysicsEngine() |
---|
168 | { |
---|
169 | this.nextBodyID = firstBodyID; |
---|
170 | this.syncRoot = new object(); |
---|
171 | this.rwLock = new AdvReaderWriterLock(); |
---|
172 | |
---|
173 | this.joints = new List<Joint>(); |
---|
174 | this.bodies = new List<Body>(); |
---|
175 | this.logics = new List<PhysicsLogic>(); |
---|
176 | |
---|
177 | this.pendingBodies = new List<Body>(); |
---|
178 | this.pendingJoints = new List<Joint>(); |
---|
179 | this.pendingLogics = new List<PhysicsLogic>(); |
---|
180 | this.pendingProxies = new List<BodyProxy>(); |
---|
181 | |
---|
182 | this.removedBodies = new List<Body>(); |
---|
183 | this.removedJoints = new List<Joint>(); |
---|
184 | this.removedLogics = new List<PhysicsLogic>(); |
---|
185 | } |
---|
186 | #endregion |
---|
187 | #region properties |
---|
188 | /// <summary> |
---|
189 | /// Gets A threadSafe List of Joints (You wont get the "The collection has changed" Exception with this) |
---|
190 | /// </summary> |
---|
191 | public ReadOnlyThreadSafeCollection<Joint> Joints |
---|
192 | { |
---|
193 | get |
---|
194 | { |
---|
195 | return new ReadOnlyThreadSafeCollection<Joint>(rwLock, joints); |
---|
196 | } |
---|
197 | } |
---|
198 | /// <summary> |
---|
199 | /// Gets A threadSafe List of Bodies (You wont get the "The collection has changed" Exception with this) |
---|
200 | /// </summary> |
---|
201 | public ReadOnlyThreadSafeCollection<Body> Bodies |
---|
202 | { |
---|
203 | get |
---|
204 | { |
---|
205 | return new ReadOnlyThreadSafeCollection<Body>(rwLock, bodies); |
---|
206 | } |
---|
207 | } |
---|
208 | /// <summary> |
---|
209 | /// Gets A threadSafe List of PhysicsLogics (You wont get the "The collection has changed" Exception with this) |
---|
210 | /// </summary> |
---|
211 | public ReadOnlyThreadSafeCollection<PhysicsLogic> Logics |
---|
212 | { |
---|
213 | get |
---|
214 | { |
---|
215 | return new ReadOnlyThreadSafeCollection<PhysicsLogic>(rwLock, logics); |
---|
216 | } |
---|
217 | } |
---|
218 | /// <summary> |
---|
219 | /// Gets and Sets The BroadPhase collision Detector. (This must be Set to a non-Null value before any calls to Update) |
---|
220 | /// </summary> |
---|
221 | public BroadPhaseCollisionDetector BroadPhase |
---|
222 | { |
---|
223 | get { return broadPhase; } |
---|
224 | set |
---|
225 | { |
---|
226 | using (rwLock.Write) |
---|
227 | { |
---|
228 | if (broadPhase != value) |
---|
229 | { |
---|
230 | if (broadPhase != null) { broadPhase.OnRemovedInternal(); } |
---|
231 | if (value != null) { value.OnAddedInternal(this); } |
---|
232 | broadPhase = value; |
---|
233 | } |
---|
234 | } |
---|
235 | } |
---|
236 | } |
---|
237 | /// <summary> |
---|
238 | /// Gets and Sets the Collision Solver (This must be Set to a non-Null value before any calls to Update) |
---|
239 | /// </summary> |
---|
240 | public CollisionSolver Solver |
---|
241 | { |
---|
242 | get |
---|
243 | { |
---|
244 | return solver; |
---|
245 | } |
---|
246 | set |
---|
247 | { |
---|
248 | using (rwLock.Write) |
---|
249 | { |
---|
250 | if (solver != value) |
---|
251 | { |
---|
252 | if (solver != null) { solver.OnRemovedInternal(); } |
---|
253 | if (value != null) { value.OnAddedInternal(this); } |
---|
254 | solver = value; |
---|
255 | } |
---|
256 | } |
---|
257 | } |
---|
258 | } |
---|
259 | /// <summary> |
---|
260 | /// The number of updates that the engine has completed. |
---|
261 | /// </summary> |
---|
262 | /// <remarks> |
---|
263 | /// This is used for making sure actions are not duplicated on a single time step. |
---|
264 | /// </remarks> |
---|
265 | public int UpdateCount { get { return updateCount; } } |
---|
266 | |
---|
267 | #endregion |
---|
268 | #region methods |
---|
269 | /// <summary> |
---|
270 | /// Adds a Body to the pending queue and will be truly added on a call to Update. |
---|
271 | /// </summary> |
---|
272 | /// <param name="item">The Body to be added.</param> |
---|
273 | public void AddBody(Body item) |
---|
274 | { |
---|
275 | PreCheckItem(item); |
---|
276 | lock (syncRoot) |
---|
277 | { |
---|
278 | CheckItem(item); |
---|
279 | item.OnPending(this); |
---|
280 | pendingBodies.Add(item); |
---|
281 | } |
---|
282 | } |
---|
283 | /// <summary> |
---|
284 | /// Adds a collection of Bodies to the pending queue and will be truly added on a call to Update. |
---|
285 | /// </summary> |
---|
286 | /// <param name="collection">The collection to be Added</param> |
---|
287 | public void AddBodyRange(ICollection<Body> collection) |
---|
288 | { |
---|
289 | if (collection == null) { throw new ArgumentNullException("collection"); } |
---|
290 | if (collection.Count == 0) { return; } |
---|
291 | lock (syncRoot) |
---|
292 | { |
---|
293 | PreCheckBodies(collection); |
---|
294 | CheckBodies(collection); |
---|
295 | BodiesOnPending(collection); |
---|
296 | pendingBodies.AddRange(collection); |
---|
297 | } |
---|
298 | } |
---|
299 | private void PreCheckBodies(ICollection<Body> collection) |
---|
300 | { |
---|
301 | foreach (Body item in collection) |
---|
302 | { |
---|
303 | PreCheckItem(item); |
---|
304 | } |
---|
305 | } |
---|
306 | private void CheckBodies(ICollection<Body> collection) |
---|
307 | { |
---|
308 | foreach (Body item in collection) |
---|
309 | { |
---|
310 | CheckItem(item); |
---|
311 | } |
---|
312 | } |
---|
313 | private void BodiesOnPending(ICollection<Body> collection) |
---|
314 | { |
---|
315 | foreach (Body item in collection) |
---|
316 | { |
---|
317 | item.OnPending(this); |
---|
318 | } |
---|
319 | } |
---|
320 | |
---|
321 | /// <summary> |
---|
322 | /// Adds a Joint to the pending queue and will be truly added on a call to Update. |
---|
323 | /// </summary> |
---|
324 | /// <param name="item">The Joint to be added.</param> |
---|
325 | public void AddJoint(Joint item) |
---|
326 | { |
---|
327 | PreCheckItem(item); |
---|
328 | lock (syncRoot) |
---|
329 | { |
---|
330 | CheckJoint(item); |
---|
331 | item.OnPendingInternal(this); |
---|
332 | pendingJoints.Add(item); |
---|
333 | } |
---|
334 | } |
---|
335 | /// <summary> |
---|
336 | /// Adds a collection of Joints to the pending queue and will be truly added on a call to Update. |
---|
337 | /// </summary> |
---|
338 | /// <param name="collection">The collection to be Added</param> |
---|
339 | public void AddJointRange(ICollection<Joint> collection) |
---|
340 | { |
---|
341 | if (collection == null) { throw new ArgumentNullException("collection"); } |
---|
342 | if (collection.Count == 0) { return; } |
---|
343 | CheckState(); |
---|
344 | lock (syncRoot) |
---|
345 | { |
---|
346 | foreach (Joint item in collection) |
---|
347 | { |
---|
348 | PreCheckItem(item); |
---|
349 | } |
---|
350 | foreach (Joint item in collection) |
---|
351 | { |
---|
352 | CheckJoint(item); |
---|
353 | } |
---|
354 | foreach (Joint item in collection) |
---|
355 | { |
---|
356 | item.OnPendingInternal(this); |
---|
357 | } |
---|
358 | pendingJoints.AddRange(collection); |
---|
359 | } |
---|
360 | } |
---|
361 | /// <summary> |
---|
362 | /// Adds a collection of Joints to the pending queue and will be truly added on a call to Update. |
---|
363 | /// </summary> |
---|
364 | /// <param name="collection">The collection to be Added</param> |
---|
365 | /// <typeparam name="T">A Type inherited from Joint</typeparam> |
---|
366 | public void AddJointRange<T>(ICollection<T> collection) |
---|
367 | where T : Joint |
---|
368 | { |
---|
369 | if (collection == null) { throw new ArgumentNullException("collection"); } |
---|
370 | if (collection.Count == 0) { return; } |
---|
371 | CheckState(); |
---|
372 | lock (syncRoot) |
---|
373 | { |
---|
374 | foreach (T item in collection) |
---|
375 | { |
---|
376 | PreCheckItem(item); |
---|
377 | } |
---|
378 | Joint[] array = new Joint[collection.Count]; |
---|
379 | int index = 0; |
---|
380 | foreach (T item in collection) |
---|
381 | { |
---|
382 | CheckJoint(item); |
---|
383 | array[index++] = item; |
---|
384 | } |
---|
385 | foreach (T item in collection) |
---|
386 | { |
---|
387 | item.OnPendingInternal(this); |
---|
388 | } |
---|
389 | pendingJoints.AddRange(array); |
---|
390 | } |
---|
391 | } |
---|
392 | |
---|
393 | /// <summary> |
---|
394 | /// Adds a PhysicsLogic to the pending queue and will be truly added on a call to Update. |
---|
395 | /// </summary> |
---|
396 | /// <param name="item">The PhysicsLogic to be added.</param> |
---|
397 | public void AddLogic(PhysicsLogic item) |
---|
398 | { |
---|
399 | PreCheckItem(item); |
---|
400 | lock (syncRoot) |
---|
401 | { |
---|
402 | ReadOnlyCollection<Body> logicBodies = item.LogicBodies; |
---|
403 | PreCheckBodies(logicBodies); |
---|
404 | CheckLogic(item); |
---|
405 | CheckBodies(logicBodies); |
---|
406 | item.OnPendingInternal(this); |
---|
407 | pendingLogics.Add(item); |
---|
408 | BodiesOnPending(logicBodies); |
---|
409 | pendingBodies.AddRange(logicBodies); |
---|
410 | } |
---|
411 | } |
---|
412 | /// <summary> |
---|
413 | /// Adds a collection of PhysicsLogics to the pending queue and will be truly added on a call to Update. |
---|
414 | /// </summary> |
---|
415 | /// <param name="collection">The collection to be Added</param> |
---|
416 | public void AddLogicRange(ICollection<PhysicsLogic> collection) |
---|
417 | { |
---|
418 | if (collection == null) { throw new ArgumentNullException("collection"); } |
---|
419 | if (collection.Count == 0) { return; } |
---|
420 | lock (syncRoot) |
---|
421 | { |
---|
422 | List<Body> logicBodies = new List<Body>(); |
---|
423 | foreach (PhysicsLogic item in collection) |
---|
424 | { |
---|
425 | PreCheckItem(item); |
---|
426 | logicBodies.AddRange(item.LogicBodies); |
---|
427 | } |
---|
428 | PreCheckBodies(logicBodies); |
---|
429 | foreach (PhysicsLogic item in collection) |
---|
430 | { |
---|
431 | CheckLogic(item); |
---|
432 | } |
---|
433 | CheckBodies(logicBodies); |
---|
434 | foreach (PhysicsLogic item in collection) |
---|
435 | { |
---|
436 | item.OnPendingInternal(this); |
---|
437 | } |
---|
438 | pendingLogics.AddRange(collection); |
---|
439 | BodiesOnPending(logicBodies); |
---|
440 | pendingBodies.AddRange(logicBodies); |
---|
441 | } |
---|
442 | } |
---|
443 | /// <summary> |
---|
444 | /// Adds a collection of PhysicsLogics to the pending queue and will be truly added on a call to Update. |
---|
445 | /// </summary> |
---|
446 | /// <param name="collection">The collection to be Added</param> |
---|
447 | /// <typeparam name="T">A Type inherited from PhysicsLogic</typeparam> |
---|
448 | public void AddLogicRange<T>(ICollection<T> collection) |
---|
449 | where T : PhysicsLogic |
---|
450 | { |
---|
451 | if (collection == null) { throw new ArgumentNullException("collection"); } |
---|
452 | if (collection.Count == 0) { return; } |
---|
453 | lock (syncRoot) |
---|
454 | { |
---|
455 | List<Body> logicBodies = new List<Body>(); |
---|
456 | foreach (T item in collection) |
---|
457 | { |
---|
458 | PreCheckItem(item); |
---|
459 | logicBodies.AddRange(item.LogicBodies); |
---|
460 | } |
---|
461 | PreCheckBodies(logicBodies); |
---|
462 | PhysicsLogic[] array = new PhysicsLogic[collection.Count]; |
---|
463 | int index = 0; |
---|
464 | foreach (T item in collection) |
---|
465 | { |
---|
466 | CheckLogic(item); |
---|
467 | array[index++] = item; |
---|
468 | } |
---|
469 | CheckBodies(logicBodies); |
---|
470 | foreach (T item in collection) |
---|
471 | { |
---|
472 | item.OnPendingInternal(this); |
---|
473 | } |
---|
474 | pendingLogics.AddRange(array); |
---|
475 | BodiesOnPending(logicBodies); |
---|
476 | pendingBodies.AddRange(logicBodies); |
---|
477 | } |
---|
478 | } |
---|
479 | /// <summary> |
---|
480 | /// Adds 2 bodies to the same proxy list. |
---|
481 | /// If they are both already part of their own proxy list then the lists will merge. |
---|
482 | /// The transformations will be calcualted automatically. |
---|
483 | /// </summary> |
---|
484 | /// <param name="body1">The first Body.</param> |
---|
485 | /// <param name="body2">The second Body.</param> |
---|
486 | /// <param name="transformation">How velocities will be transformed from body1 to body2.</param> |
---|
487 | /// <remarks> |
---|
488 | /// This will most likely be removed if i ever figure out how to make a joint like this. |
---|
489 | /// </remarks> |
---|
490 | public void AddProxy(Body body1, Body body2, Matrix2x2 transformation) |
---|
491 | { |
---|
492 | if (body1 == null) { throw new ArgumentNullException("body1"); } |
---|
493 | if (body2 == null) { throw new ArgumentNullException("body2"); } |
---|
494 | if (body1 == body2) { throw new ArgumentException("They cannot be the same body"); } |
---|
495 | lock (syncRoot) |
---|
496 | { |
---|
497 | pendingProxies.Add(new BodyProxy(body1, body2, transformation)); |
---|
498 | } |
---|
499 | } |
---|
500 | /// <summary> |
---|
501 | /// Updates the Engine with a change in time. |
---|
502 | /// This call will block all access to the engine while it is running. |
---|
503 | /// A complete call to this method is also known as a timestep. |
---|
504 | /// </summary> |
---|
505 | /// <param name="dt">The change in time since the last call to this method. (In Seconds)</param> |
---|
506 | public void Update(Scalar dt/*,Scalar trueDt*/) |
---|
507 | { |
---|
508 | if (dt < 0) { throw new ArgumentOutOfRangeException("dt"); } |
---|
509 | CheckState(); |
---|
510 | rwLock.EnterWrite(); |
---|
511 | TimeStep step = new TimeStep(dt,/*trueDt,*/ updateCount); |
---|
512 | inUpdate = true; |
---|
513 | try |
---|
514 | { |
---|
515 | RemoveExpired(); |
---|
516 | AddPending(); |
---|
517 | if (logicsNeedSorting) |
---|
518 | { |
---|
519 | logicsNeedSorting = false; |
---|
520 | logics.Sort(logicComparer); |
---|
521 | } |
---|
522 | UpdateTime(step); |
---|
523 | solver.Solve(step); |
---|
524 | OnPositionChanged(); |
---|
525 | } |
---|
526 | finally |
---|
527 | { |
---|
528 | updateCount++; |
---|
529 | inUpdate = false; |
---|
530 | rwLock.ExitWrite(); |
---|
531 | } |
---|
532 | } |
---|
533 | |
---|
534 | /// <summary> |
---|
535 | /// Clears the Engine of all objects. Also clears the Detector and Solver. |
---|
536 | /// </summary> |
---|
537 | public void Clear() |
---|
538 | { |
---|
539 | rwLock.EnterWrite(); |
---|
540 | try |
---|
541 | { |
---|
542 | ClearPending(); |
---|
543 | ClearAdded(); |
---|
544 | updateCount = 0; |
---|
545 | nextBodyID = firstBodyID; |
---|
546 | } |
---|
547 | finally |
---|
548 | { |
---|
549 | rwLock.ExitWrite(); |
---|
550 | } |
---|
551 | } |
---|
552 | |
---|
553 | private void ClearPending() |
---|
554 | { |
---|
555 | List<Body> pendingBodies; |
---|
556 | List<Joint> pendingJoints; |
---|
557 | List<PhysicsLogic> pendingLogics; |
---|
558 | lock (syncRoot) |
---|
559 | { |
---|
560 | pendingBodies = this.pendingBodies; |
---|
561 | this.pendingBodies = new List<Body>(); |
---|
562 | pendingJoints = this.pendingJoints; |
---|
563 | this.pendingJoints = new List<Joint>(); |
---|
564 | pendingLogics = this.pendingLogics; |
---|
565 | this.pendingLogics = new List<PhysicsLogic>(); |
---|
566 | pendingProxies.Clear(); |
---|
567 | } |
---|
568 | foreach (Body body in pendingBodies) |
---|
569 | { |
---|
570 | body.OnRemoved(); |
---|
571 | } |
---|
572 | pendingBodies.Clear(); |
---|
573 | foreach (Joint joint in pendingJoints) |
---|
574 | { |
---|
575 | joint.OnRemovedInternal(); |
---|
576 | } |
---|
577 | pendingJoints.Clear(); |
---|
578 | foreach (PhysicsLogic logic in pendingLogics) |
---|
579 | { |
---|
580 | logic.OnRemovedInternal(); |
---|
581 | } |
---|
582 | pendingLogics.Clear(); |
---|
583 | } |
---|
584 | private void ClearAdded() |
---|
585 | { |
---|
586 | solver.Clear(); |
---|
587 | broadPhase.Clear(); |
---|
588 | foreach (Body body in bodies) |
---|
589 | { |
---|
590 | body.OnRemoved(); |
---|
591 | } |
---|
592 | foreach (Joint joint in joints) |
---|
593 | { |
---|
594 | joint.OnRemovedInternal(); |
---|
595 | } |
---|
596 | foreach (PhysicsLogic logic in logics) |
---|
597 | { |
---|
598 | logic.OnRemovedInternal(); |
---|
599 | } |
---|
600 | if (BodiesRemoved != null && bodies.Count > 0) |
---|
601 | { |
---|
602 | BodiesRemoved(this, new CollectionEventArgs<Body>(bodies.AsReadOnly())); |
---|
603 | } |
---|
604 | if (JointsRemoved != null && joints.Count > 0) |
---|
605 | { |
---|
606 | JointsRemoved(this, new CollectionEventArgs<Joint>(joints.AsReadOnly())); |
---|
607 | } |
---|
608 | if (LogicsRemoved != null && logics.Count > 0) |
---|
609 | { |
---|
610 | LogicsRemoved(this, new CollectionEventArgs<PhysicsLogic>(logics.AsReadOnly())); |
---|
611 | } |
---|
612 | bodies.Clear(); |
---|
613 | joints.Clear(); |
---|
614 | logics.Clear(); |
---|
615 | } |
---|
616 | |
---|
617 | private void UpdateTime(TimeStep step) |
---|
618 | { |
---|
619 | for (int index = 0; index < bodies.Count; ++index) |
---|
620 | { |
---|
621 | bodies[index].UpdateTime(step); |
---|
622 | } |
---|
623 | for (int index = 0; index < joints.Count; ++index) |
---|
624 | { |
---|
625 | joints[index].UpdateTime(step); |
---|
626 | } |
---|
627 | for (int index = 0; index < logics.Count; ++index) |
---|
628 | { |
---|
629 | logics[index].UpdateTime(step); |
---|
630 | } |
---|
631 | if (Updated != null) { Updated(this, new UpdatedEventArgs(step)); } |
---|
632 | } |
---|
633 | private void OnPositionChanged() |
---|
634 | { |
---|
635 | int count = bodies.Count; |
---|
636 | for (int index = 0; index < count; ++index) |
---|
637 | { |
---|
638 | bodies[index].OnPositionChanged(); |
---|
639 | } |
---|
640 | } |
---|
641 | |
---|
642 | private void RemoveExpired() |
---|
643 | { |
---|
644 | RemoveExpiredBodies(); |
---|
645 | RemoveExpiredJoints(); |
---|
646 | RemoveExpiredLogics(); |
---|
647 | } |
---|
648 | private void RemoveExpiredBodies() |
---|
649 | { |
---|
650 | if (bodies.RemoveAll(IsBodyExpired) == 0) { return; } |
---|
651 | solver.RemoveExpiredBodies(); |
---|
652 | broadPhase.RemoveExpiredBodies(); |
---|
653 | for (int index = 0; index < logics.Count; ++index) |
---|
654 | { |
---|
655 | logics[index].RemoveExpiredBodies(); |
---|
656 | } |
---|
657 | if (BodiesRemoved != null) |
---|
658 | { |
---|
659 | BodiesRemoved(this, new CollectionEventArgs<Body>(removedBodies.AsReadOnly())); |
---|
660 | removedBodies.Clear(); |
---|
661 | } |
---|
662 | } |
---|
663 | private void RemoveExpiredJoints() |
---|
664 | { |
---|
665 | if (joints.RemoveAll(IsJointExpired) == 0) { return; } |
---|
666 | solver.RemoveExpiredJoints(); |
---|
667 | if (JointsRemoved != null) |
---|
668 | { |
---|
669 | JointsRemoved(this, new CollectionEventArgs<Joint>(removedJoints.AsReadOnly())); |
---|
670 | removedJoints.Clear(); |
---|
671 | } |
---|
672 | } |
---|
673 | private void RemoveExpiredLogics() |
---|
674 | { |
---|
675 | if (logics.RemoveAll(IsLogicExpired) == 0) { return; } |
---|
676 | if (LogicsRemoved != null) |
---|
677 | { |
---|
678 | LogicsRemoved(this, new CollectionEventArgs<PhysicsLogic>(removedLogics.AsReadOnly())); |
---|
679 | removedLogics.Clear(); |
---|
680 | } |
---|
681 | } |
---|
682 | |
---|
683 | private bool IsBodyExpired(Body body) |
---|
684 | { |
---|
685 | if (!body.Lifetime.IsExpired) { return false; } |
---|
686 | if (BodiesRemoved != null) { removedBodies.Add(body); } |
---|
687 | body.OnRemoved(); |
---|
688 | return true; |
---|
689 | } |
---|
690 | private bool IsJointExpired(Joint joint) |
---|
691 | { |
---|
692 | if (!joint.Lifetime.IsExpired) { return false; } |
---|
693 | if (JointsRemoved != null) { removedJoints.Add(joint); } |
---|
694 | joint.OnRemovedInternal(); |
---|
695 | return true; |
---|
696 | } |
---|
697 | private bool IsLogicExpired(PhysicsLogic logic) |
---|
698 | { |
---|
699 | if (!logic.Lifetime.IsExpired) { return false; } |
---|
700 | if (LogicsRemoved != null) { removedLogics.Add(logic); } |
---|
701 | logic.OnRemovedInternal(); |
---|
702 | return true; |
---|
703 | } |
---|
704 | |
---|
705 | private void AddPending() |
---|
706 | { |
---|
707 | lock (syncRoot) |
---|
708 | { |
---|
709 | if (pendingBodies.Count > 0) { AddPendingBodies(); } |
---|
710 | if (pendingJoints.Count > 0) { AddPendingJoints(); } |
---|
711 | if (pendingLogics.Count > 0) { AddPendingLogics(); } |
---|
712 | if (pendingProxies.Count > 0) { AddPendingProxies(); } |
---|
713 | } |
---|
714 | } |
---|
715 | private void AddPendingBodies() |
---|
716 | { |
---|
717 | for (int index = 0; index < pendingBodies.Count; ++index) |
---|
718 | { |
---|
719 | Body item = pendingBodies[index]; |
---|
720 | item.ID = nextBodyID++; |
---|
721 | item.ApplyPosition(); |
---|
722 | } |
---|
723 | bodies.AddRange(pendingBodies); |
---|
724 | solver.AddBodyRange(pendingBodies); |
---|
725 | broadPhase.AddBodyRange(pendingBodies); |
---|
726 | for (int index = 0; index < logics.Count; ++index) |
---|
727 | { |
---|
728 | logics[index].AddBodyRange(pendingBodies); |
---|
729 | } |
---|
730 | for (int index = 0; index < pendingBodies.Count; ++index) |
---|
731 | { |
---|
732 | pendingBodies[index].OnAdded(); |
---|
733 | } |
---|
734 | if (BodiesAdded != null) { BodiesAdded(this, new CollectionEventArgs<Body>(pendingBodies.AsReadOnly())); } |
---|
735 | pendingBodies.Clear(); |
---|
736 | } |
---|
737 | private void AddPendingJoints() |
---|
738 | { |
---|
739 | joints.AddRange(pendingJoints); |
---|
740 | solver.AddJointRange(pendingJoints); |
---|
741 | for (int index = 0; index < pendingJoints.Count; ++index) |
---|
742 | { |
---|
743 | Joint item = pendingJoints[index]; |
---|
744 | item.OnAddedInternal(); |
---|
745 | } |
---|
746 | if (JointsAdded != null) { JointsAdded(this, new CollectionEventArgs<Joint>(pendingJoints.AsReadOnly())); } |
---|
747 | pendingJoints.Clear(); |
---|
748 | } |
---|
749 | private void AddPendingLogics() |
---|
750 | { |
---|
751 | logics.AddRange(pendingLogics); |
---|
752 | for (int index = 0; index < pendingLogics.Count; ++index) |
---|
753 | { |
---|
754 | PhysicsLogic logic = pendingLogics[index]; |
---|
755 | logic.OnAddedInternal(); |
---|
756 | } |
---|
757 | if (LogicsAdded != null) { LogicsAdded(this, new CollectionEventArgs<PhysicsLogic>(pendingLogics.AsReadOnly())); } |
---|
758 | pendingLogics.Clear(); |
---|
759 | this.logicsNeedSorting = true; |
---|
760 | } |
---|
761 | private void AddPendingProxies() |
---|
762 | { |
---|
763 | for (int index = 0; index < pendingProxies.Count; ++index) |
---|
764 | { |
---|
765 | BodyProxy proxy = pendingProxies[index]; |
---|
766 | Body.CreateProxy(proxy.Body1, proxy.Body2, proxy.transformation); |
---|
767 | } |
---|
768 | pendingProxies.Clear(); |
---|
769 | } |
---|
770 | private void CheckState() |
---|
771 | { |
---|
772 | if (this.broadPhase == null) { throw new InvalidOperationException("The BroadPhase property must be set."); } |
---|
773 | if (this.solver == null) { throw new InvalidOperationException("The Solver property must be set."); } |
---|
774 | } |
---|
775 | private void CheckJoint(Joint joint) |
---|
776 | { |
---|
777 | CheckItem(joint); |
---|
778 | joint.BeforeAddCheckInternal(this); |
---|
779 | solver.CheckJoint(joint); |
---|
780 | } |
---|
781 | private void CheckLogic(PhysicsLogic logic) |
---|
782 | { |
---|
783 | CheckItem(logic); |
---|
784 | logic.BeforeAddCheck(this); |
---|
785 | } |
---|
786 | internal void RunLogic(TimeStep step) |
---|
787 | { |
---|
788 | for (int index = 0; index < logics.Count; ++index) |
---|
789 | { |
---|
790 | logics[index].RunLogic(step); |
---|
791 | } |
---|
792 | } |
---|
793 | internal void HandleCollision(TimeStep step, Body body1, Body body2) |
---|
794 | { |
---|
795 | if (body1.Mass.MassInv == 0 && body2.Mass.MassInv == 0) { return; } |
---|
796 | IShape shape1 = body1.Shape; |
---|
797 | IShape shape2 = body2.Shape; |
---|
798 | |
---|
799 | |
---|
800 | if (shape1.CanGetCustomIntersection || |
---|
801 | shape2.CanGetCustomIntersection || |
---|
802 | body1.IsBroadPhaseOnly || |
---|
803 | body2.IsBroadPhaseOnly) |
---|
804 | { |
---|
805 | object customIntersectionInfo; |
---|
806 | if (body1.IsBroadPhaseOnly) |
---|
807 | { |
---|
808 | body1.OnCollision(step,body2, null); |
---|
809 | } |
---|
810 | else if (shape1.CanGetCustomIntersection && |
---|
811 | shape1.TryGetCustomIntersection(body1, body2, out customIntersectionInfo)) |
---|
812 | { |
---|
813 | body1.OnCollision(step, body2, customIntersectionInfo); |
---|
814 | } |
---|
815 | if (body2.IsBroadPhaseOnly) |
---|
816 | { |
---|
817 | body2.OnCollision(step, body1, null); |
---|
818 | } |
---|
819 | else if (shape2.CanGetCustomIntersection && |
---|
820 | shape2.TryGetCustomIntersection(body2, body1, out customIntersectionInfo)) |
---|
821 | { |
---|
822 | body2.OnCollision(step, body1, customIntersectionInfo); |
---|
823 | } |
---|
824 | } |
---|
825 | else |
---|
826 | { |
---|
827 | IContact contact; |
---|
828 | if (solver.TryGetIntersection(step, body1, body2, out contact) ) |
---|
829 | { |
---|
830 | if ( contact.State == ContactState.New ) |
---|
831 | { |
---|
832 | body1.OnCollision( step, body2, contact ); |
---|
833 | body2.OnCollision( step, body1, contact ); |
---|
834 | } |
---|
835 | |
---|
836 | body1.OnColliding( step, body2, contact ); |
---|
837 | body2.OnColliding( step, body1, contact ); |
---|
838 | } |
---|
839 | } |
---|
840 | |
---|
841 | } |
---|
842 | #endregion |
---|
843 | } |
---|
844 | } |
---|