axiosengine/axios/Dynamics/World.cs

1456 lines
52 KiB
C#
Raw Normal View History

2012-03-19 23:57:59 +00:00
/*
* Farseer Physics Engine based on Box2D.XNA port:
* Copyright (c) 2010 Ian Qvist
*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
using System;
using System.Collections.Generic;
using System.Diagnostics;
using FarseerPhysics.Collision;
using FarseerPhysics.Common;
using FarseerPhysics.Controllers;
using FarseerPhysics.Dynamics.Contacts;
using FarseerPhysics.Dynamics.Joints;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics
{
/// <summary>
/// Contains filter data that can determine whether an object should be processed or not.
/// </summary>
public abstract class FilterData
{
public Category DisabledOnCategories = Category.None;
public int DisabledOnGroup;
public Category EnabledOnCategories = Category.All;
public int EnabledOnGroup;
public virtual bool IsActiveOn(Body body)
{
if (body == null || !body.Enabled || body.IsStatic)
return false;
if (body.FixtureList == null)
return false;
foreach (Fixture fixture in body.FixtureList)
{
//Disable
if ((fixture.CollisionGroup == DisabledOnGroup) &&
fixture.CollisionGroup != 0 && DisabledOnGroup != 0)
return false;
if ((fixture.CollisionCategories & DisabledOnCategories) != Category.None)
return false;
if (EnabledOnGroup != 0 || EnabledOnCategories != Category.All)
{
//Enable
if ((fixture.CollisionGroup == EnabledOnGroup) &&
fixture.CollisionGroup != 0 && EnabledOnGroup != 0)
return true;
if ((fixture.CollisionCategories & EnabledOnCategories) != Category.None &&
EnabledOnCategories != Category.All)
return true;
}
else
{
return true;
}
}
return false;
}
/// <summary>
/// Adds the category.
/// </summary>
/// <param name="category">The category.</param>
public void AddDisabledCategory(Category category)
{
DisabledOnCategories |= category;
}
/// <summary>
/// Removes the category.
/// </summary>
/// <param name="category">The category.</param>
public void RemoveDisabledCategory(Category category)
{
DisabledOnCategories &= ~category;
}
/// <summary>
/// Determines whether this body ignores the the specified controller.
/// </summary>
/// <param name="category">The category.</param>
/// <returns>
/// <c>true</c> if the object has the specified category; otherwise, <c>false</c>.
/// </returns>
public bool IsInDisabledCategory(Category category)
{
return (DisabledOnCategories & category) == category;
}
/// <summary>
/// Adds the category.
/// </summary>
/// <param name="category">The category.</param>
public void AddEnabledCategory(Category category)
{
EnabledOnCategories |= category;
}
/// <summary>
/// Removes the category.
/// </summary>
/// <param name="category">The category.</param>
public void RemoveEnabledCategory(Category category)
{
EnabledOnCategories &= ~category;
}
/// <summary>
/// Determines whether this body ignores the the specified controller.
/// </summary>
/// <param name="category">The category.</param>
/// <returns>
/// <c>true</c> if the object has the specified category; otherwise, <c>false</c>.
/// </returns>
public bool IsInEnabledCategory(Category category)
{
return (EnabledOnCategories & category) == category;
}
}
[Flags]
public enum WorldFlags
{
/// <summary>
/// Flag that indicates a new fixture has been added to the world.
/// </summary>
NewFixture = (1 << 0),
/// <summary>
/// Flag that clear the forces after each time step.
/// </summary>
ClearForces = (1 << 2),
SubStepping = (1 << 4),
}
/// <summary>
/// The world class manages all physics entities, dynamic simulation,
/// and asynchronous queries.
/// </summary>
public class World
{
/// <summary>
/// Fires whenever a body has been added
/// </summary>
public BodyDelegate BodyAdded;
/// <summary>
/// Fires whenever a body has been removed
/// </summary>
public BodyDelegate BodyRemoved;
internal Queue<Contact> ContactPool = new Queue<Contact>(256);
/// <summary>
/// Fires whenever a fixture has been added
/// </summary>
public FixtureDelegate FixtureAdded;
/// <summary>
/// Fires whenever a fixture has been removed
/// </summary>
public FixtureDelegate FixtureRemoved;
internal WorldFlags Flags;
/// <summary>
/// Fires whenever a joint has been added
/// </summary>
public JointDelegate JointAdded;
/// <summary>
/// Fires whenever a joint has been removed
/// </summary>
public JointDelegate JointRemoved;
public ControllerDelegate ControllerAdded;
public ControllerDelegate ControllerRemoved;
private float _invDt0;
public Island Island = new Island();
private Body[] _stack = new Body[64];
private bool _stepComplete;
private HashSet<Body> _bodyAddList = new HashSet<Body>();
private HashSet<Body> _bodyRemoveList = new HashSet<Body>();
private HashSet<Joint> _jointAddList = new HashSet<Joint>();
private HashSet<Joint> _jointRemoveList = new HashSet<Joint>();
private TOIInput _input = new TOIInput();
/// <summary>
/// If false, the whole simulation stops. It still processes added and removed geometries.
/// </summary>
public bool Enabled = true;
#if (!SILVERLIGHT)
private Stopwatch _watch = new Stopwatch();
#endif
/// <summary>
/// Initializes a new instance of the <see cref="World"/> class.
/// </summary>
private World()
{
Flags = WorldFlags.ClearForces;
ControllerList = new List<Controller>();
BreakableBodyList = new List<BreakableBody>();
BodyList = new List<Body>(32);
JointList = new List<Joint>(32);
}
public World(Vector2 gravity, AABB span)
: this()
{
Gravity = gravity;
ContactManager = new ContactManager(new QuadTreeBroadPhase(span));
}
/// <summary>
/// Initializes a new instance of the <see cref="World"/> class.
/// </summary>
/// <param name="gravity">The gravity.</param>
public World(Vector2 gravity)
: this()
{
ContactManager = new ContactManager(new DynamicTreeBroadPhase());
Gravity = gravity;
}
public List<Controller> ControllerList { get; private set; }
public List<BreakableBody> BreakableBodyList { get; private set; }
public float UpdateTime { get; private set; }
public float ContinuousPhysicsTime { get; private set; }
public float ControllersUpdateTime { get; private set; }
public float AddRemoveTime { get; private set; }
public float ContactsUpdateTime { get; private set; }
public float SolveUpdateTime { get; private set; }
/// <summary>
/// Get the number of broad-phase proxies.
/// </summary>
/// <value>The proxy count.</value>
public int ProxyCount
{
get { return ContactManager.BroadPhase.ProxyCount; }
}
/// <summary>
/// Change the global gravity vector.
/// </summary>
/// <value>The gravity.</value>
public Vector2 Gravity;
/// <summary>
/// Set flag to control automatic clearing of forces after each time step.
/// </summary>
/// <value><c>true</c> if it should auto clear forces; otherwise, <c>false</c>.</value>
public bool AutoClearForces
{
set
{
if (value)
{
Flags |= WorldFlags.ClearForces;
}
else
{
Flags &= ~WorldFlags.ClearForces;
}
}
get { return (Flags & WorldFlags.ClearForces) == WorldFlags.ClearForces; }
}
/// <summary>
/// Get the contact manager for testing.
/// </summary>
/// <value>The contact manager.</value>
public ContactManager ContactManager { get; private set; }
/// <summary>
/// Get the world body list.
/// </summary>
/// <value>Thehead of the world body list.</value>
public List<Body> BodyList { get; private set; }
/// <summary>
/// Get the world joint list.
/// </summary>
/// <value>The joint list.</value>
public List<Joint> JointList { get; private set; }
/// <summary>
/// Get the world contact list. With the returned contact, use Contact.GetNext to get
/// the next contact in the world list. A null contact indicates the end of the list.
/// </summary>
/// <value>The head of the world contact list.</value>
public List<Contact> ContactList
{
get { return ContactManager.ContactList; }
}
/// <summary>
/// Enable/disable single stepped continuous physics. For testing.
/// </summary>
public bool EnableSubStepping
{
set
{
if (value)
{
Flags |= WorldFlags.SubStepping;
}
else
{
Flags &= ~WorldFlags.SubStepping;
}
}
get { return (Flags & WorldFlags.SubStepping) == WorldFlags.SubStepping; }
}
/// <summary>
/// Add a rigid body.
/// </summary>
/// <returns></returns>
internal void AddBody(Body body)
{
Debug.Assert(!_bodyAddList.Contains(body), "You are adding the same body more than once.");
if (!_bodyAddList.Contains(body))
_bodyAddList.Add(body);
}
/// <summary>
/// Destroy a rigid body.
/// Warning: This automatically deletes all associated shapes and joints.
/// </summary>
/// <param name="body">The body.</param>
public void RemoveBody(Body body)
{
Debug.Assert(!_bodyRemoveList.Contains(body),
"The body is already marked for removal. You are removing the body more than once.");
if (!_bodyRemoveList.Contains(body))
_bodyRemoveList.Add(body);
}
/// <summary>
/// Create a joint to constrain bodies together. This may cause the connected bodies to cease colliding.
/// </summary>
/// <param name="joint">The joint.</param>
public void AddJoint(Joint joint)
{
Debug.Assert(!_jointAddList.Contains(joint), "You are adding the same joint more than once.");
if (!_jointAddList.Contains(joint))
_jointAddList.Add(joint);
}
private void RemoveJoint(Joint joint, bool doCheck)
{
if (doCheck)
{
Debug.Assert(!_jointRemoveList.Contains(joint),
"The joint is already marked for removal. You are removing the joint more than once.");
}
if (!_jointRemoveList.Contains(joint))
_jointRemoveList.Add(joint);
}
/// <summary>
/// Destroy a joint. This may cause the connected bodies to begin colliding.
/// </summary>
/// <param name="joint">The joint.</param>
public void RemoveJoint(Joint joint)
{
RemoveJoint(joint, true);
}
/// <summary>
/// All adds and removes are cached by the World duing a World step.
/// To process the changes before the world updates again, call this method.
/// </summary>
public void ProcessChanges()
{
ProcessAddedBodies();
ProcessAddedJoints();
ProcessRemovedBodies();
ProcessRemovedJoints();
}
private void ProcessRemovedJoints()
{
if (_jointRemoveList.Count > 0)
{
foreach (Joint joint in _jointRemoveList)
{
bool collideConnected = joint.CollideConnected;
// Remove from the world list.
JointList.Remove(joint);
// Disconnect from island graph.
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
// Wake up connected bodies.
bodyA.Awake = true;
// WIP David
if (!joint.IsFixedType())
{
bodyB.Awake = true;
}
// Remove from body 1.
if (joint.EdgeA.Prev != null)
{
joint.EdgeA.Prev.Next = joint.EdgeA.Next;
}
if (joint.EdgeA.Next != null)
{
joint.EdgeA.Next.Prev = joint.EdgeA.Prev;
}
if (joint.EdgeA == bodyA.JointList)
{
bodyA.JointList = joint.EdgeA.Next;
}
joint.EdgeA.Prev = null;
joint.EdgeA.Next = null;
// WIP David
if (!joint.IsFixedType())
{
// Remove from body 2
if (joint.EdgeB.Prev != null)
{
joint.EdgeB.Prev.Next = joint.EdgeB.Next;
}
if (joint.EdgeB.Next != null)
{
joint.EdgeB.Next.Prev = joint.EdgeB.Prev;
}
if (joint.EdgeB == bodyB.JointList)
{
bodyB.JointList = joint.EdgeB.Next;
}
joint.EdgeB.Prev = null;
joint.EdgeB.Next = null;
}
// WIP David
if (!joint.IsFixedType())
{
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.Other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.Contact.FlagForFiltering();
}
edge = edge.Next;
}
}
}
if (JointRemoved != null)
{
JointRemoved(joint);
}
}
_jointRemoveList.Clear();
}
}
private void ProcessAddedJoints()
{
if (_jointAddList.Count > 0)
{
foreach (Joint joint in _jointAddList)
{
// Connect to the world list.
JointList.Add(joint);
// Connect to the bodies' doubly linked lists.
joint.EdgeA.Joint = joint;
joint.EdgeA.Other = joint.BodyB;
joint.EdgeA.Prev = null;
joint.EdgeA.Next = joint.BodyA.JointList;
if (joint.BodyA.JointList != null)
joint.BodyA.JointList.Prev = joint.EdgeA;
joint.BodyA.JointList = joint.EdgeA;
// WIP David
if (!joint.IsFixedType())
{
joint.EdgeB.Joint = joint;
joint.EdgeB.Other = joint.BodyA;
joint.EdgeB.Prev = null;
joint.EdgeB.Next = joint.BodyB.JointList;
if (joint.BodyB.JointList != null)
joint.BodyB.JointList.Prev = joint.EdgeB;
joint.BodyB.JointList = joint.EdgeB;
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
// If the joint prevents collisions, then flag any contacts for filtering.
if (joint.CollideConnected == false)
{
ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.Other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.Contact.FlagForFiltering();
}
edge = edge.Next;
}
}
}
if (JointAdded != null)
JointAdded(joint);
// Note: creating a joint doesn't wake the bodies.
}
_jointAddList.Clear();
}
}
private void ProcessAddedBodies()
{
if (_bodyAddList.Count > 0)
{
foreach (Body body in _bodyAddList)
{
// Add to world list.
BodyList.Add(body);
if (BodyAdded != null)
BodyAdded(body);
}
_bodyAddList.Clear();
}
}
private void ProcessRemovedBodies()
{
if (_bodyRemoveList.Count > 0)
{
foreach (Body body in _bodyRemoveList)
{
Debug.Assert(BodyList.Count > 0);
// You tried to remove a body that is not contained in the BodyList.
// Are you removing the body more than once?
Debug.Assert(BodyList.Contains(body));
// Delete the attached joints.
JointEdge je = body.JointList;
while (je != null)
{
JointEdge je0 = je;
je = je.Next;
RemoveJoint(je0.Joint, false);
}
body.JointList = null;
// Delete the attached contacts.
ContactEdge ce = body.ContactList;
while (ce != null)
{
ContactEdge ce0 = ce;
ce = ce.Next;
ContactManager.Destroy(ce0.Contact);
}
body.ContactList = null;
// Delete the attached fixtures. This destroys broad-phase proxies.
for (int i = 0; i < body.FixtureList.Count; i++)
{
body.FixtureList[i].DestroyProxies(ContactManager.BroadPhase);
body.FixtureList[i].Destroy();
}
body.FixtureList = null;
// Remove world body list.
BodyList.Remove(body);
if (BodyRemoved != null)
BodyRemoved(body);
}
_bodyRemoveList.Clear();
}
}
/// <summary>
/// Take a time step. This performs collision detection, integration,
/// and consraint solution.
/// </summary>
/// <param name="dt">The amount of time to simulate, this should not vary.</param>
public void Step(float dt)
{
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
_watch.Start();
#endif
ProcessChanges();
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
AddRemoveTime = _watch.ElapsedTicks;
#endif
//If there is no change in time, no need to calculate anything.
if (dt == 0 || !Enabled)
{
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_watch.Reset();
}
#endif
return;
}
// If new fixtures were added, we need to find the new contacts.
if ((Flags & WorldFlags.NewFixture) == WorldFlags.NewFixture)
{
ContactManager.FindNewContacts();
Flags &= ~WorldFlags.NewFixture;
}
TimeStep step;
step.inv_dt = 1.0f / dt;
step.dt = dt;
step.dtRatio = _invDt0 * dt;
//Update controllers
for (int i = 0; i < ControllerList.Count; i++)
{
ControllerList[i].Update(dt);
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
ControllersUpdateTime = _watch.ElapsedTicks - AddRemoveTime;
#endif
// Update contacts. This is where some contacts are destroyed.
ContactManager.Collide();
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
ContactsUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime);
#endif
// Integrate velocities, solve velocity raints, and integrate positions.
Solve(ref step);
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
SolveUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime);
#endif
// Handle TOI events.
if (Settings.ContinuousPhysics)
{
SolveTOI(ref step);
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
ContinuousPhysicsTime = _watch.ElapsedTicks -
(AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime + SolveUpdateTime);
#endif
_invDt0 = step.inv_dt;
if ((Flags & WorldFlags.ClearForces) != 0)
{
ClearForces();
}
for (int i = 0; i < BreakableBodyList.Count; i++)
{
BreakableBodyList[i].Update();
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
//AddRemoveTime = 1000 * AddRemoveTime / Stopwatch.Frequency;
UpdateTime = _watch.ElapsedTicks;
_watch.Reset();
}
#endif
}
/// <summary>
/// Call this after you are done with time steps to clear the forces. You normally
/// call this after each call to Step, unless you are performing sub-steps. By default,
/// forces will be automatically cleared, so you don't need to call this function.
/// </summary>
public void ClearForces()
{
for (int i = 0; i < BodyList.Count; i++)
{
Body body = BodyList[i];
body.Force = Vector2.Zero;
body.Torque = 0.0f;
}
}
/// <summary>
/// Query the world for all fixtures that potentially overlap the
/// provided AABB.
///
/// Inside the callback:
/// Return true: Continues the query
/// Return false: Terminate the query
/// </summary>
/// <param name="callback">A user implemented callback class.</param>
/// <param name="aabb">The aabb query box.</param>
public void QueryAABB(Func<Fixture, bool> callback, ref AABB aabb)
{
ContactManager.BroadPhase.Query(proxyId =>
{
FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId);
return callback(proxy.Fixture);
}, ref aabb);
}
/// <summary>
/// Ray-cast the world for all fixtures in the path of the ray. Your callback
/// controls whether you get the closest point, any point, or n-points.
/// The ray-cast ignores shapes that contain the starting point.
///
/// Inside the callback:
/// return -1: ignore this fixture and continue
/// return 0: terminate the ray cast
/// return fraction: clip the ray to this point
/// return 1: don't clip the ray and continue
/// </summary>
/// <param name="callback">A user implemented callback class.</param>
/// <param name="point1">The ray starting point.</param>
/// <param name="point2">The ray ending point.</param>
public void RayCast(RayCastCallback callback, Vector2 point1, Vector2 point2)
{
RayCastInput input = new RayCastInput();
input.MaxFraction = 1.0f;
input.Point1 = point1;
input.Point2 = point2;
ContactManager.BroadPhase.RayCast((rayCastInput, proxyId) =>
{
FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId);
Fixture fixture = proxy.Fixture;
int index = proxy.ChildIndex;
RayCastOutput output;
bool hit = fixture.RayCast(out output, ref rayCastInput, index);
if (hit)
{
float fraction = output.Fraction;
Vector2 point = (1.0f - fraction) * input.Point1 +
fraction * input.Point2;
return callback(fixture, point, output.Normal, fraction);
}
return input.MaxFraction;
}, ref input);
}
private void Solve(ref TimeStep step)
{
// Size the island for the worst case.
Island.Reset(BodyList.Count,
ContactManager.ContactList.Count,
JointList.Count,
ContactManager);
// Clear all the island flags.
foreach (Body b in BodyList)
{
b.Flags &= ~BodyFlags.Island;
}
for (int i = 0; i < ContactManager.ContactList.Count; i++)
{
Contact c = ContactManager.ContactList[i];
c.Flags &= ~ContactFlags.Island;
}
foreach (Joint j in JointList)
{
j.IslandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = BodyList.Count;
if (stackSize > _stack.Length)
_stack = new Body[Math.Max(_stack.Length * 2, stackSize)];
for (int index = BodyList.Count - 1; index >= 0; index--)
{
Body seed = BodyList[index];
if ((seed.Flags & (BodyFlags.Island)) != BodyFlags.None)
{
continue;
}
if (seed.Awake == false || seed.Enabled == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.BodyType == BodyType.Static)
{
continue;
}
// Reset island and stack.
Island.Clear();
int stackCount = 0;
_stack[stackCount++] = seed;
seed.Flags |= BodyFlags.Island;
// Perform a depth first search (DFS) on the constraint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = _stack[--stackCount];
Debug.Assert(b.Enabled);
Island.Add(b);
// Make sure the body is awake.
b.Awake = true;
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.BodyType == BodyType.Static)
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge ce = b.ContactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
// Has this contact already been added to an island?
if ((contact.Flags & ContactFlags.Island) != ContactFlags.None)
{
continue;
}
// Is this contact solid and touching?
if (!ce.Contact.Enabled || !ce.Contact.IsTouching())
{
continue;
}
// Skip sensors.
bool sensorA = contact.FixtureA.IsSensor;
bool sensorB = contact.FixtureB.IsSensor;
if (sensorA || sensorB)
{
continue;
}
Island.Add(contact);
contact.Flags |= ContactFlags.Island;
Body other = ce.Other;
// Was the other body already added to this island?
if ((other.Flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
Debug.Assert(stackCount < stackSize);
_stack[stackCount++] = other;
other.Flags |= BodyFlags.Island;
}
// Search all joints connect to this body.
for (JointEdge je = b.JointList; je != null; je = je.Next)
{
if (je.Joint.IslandFlag)
{
continue;
}
Body other = je.Other;
// WIP David
//Enter here when it's a non-fixed joint. Non-fixed joints have a other body.
if (other != null)
{
// Don't simulate joints connected to inactive bodies.
if (other.Enabled == false)
{
continue;
}
Island.Add(je.Joint);
je.Joint.IslandFlag = true;
if ((other.Flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
Debug.Assert(stackCount < stackSize);
_stack[stackCount++] = other;
other.Flags |= BodyFlags.Island;
}
else
{
Island.Add(je.Joint);
je.Joint.IslandFlag = true;
}
}
}
Island.Solve(ref step, ref Gravity);
// Post solve cleanup.
for (int i = 0; i < Island.BodyCount; ++i)
{
// Allow static bodies to participate in other islands.
Body b = Island.Bodies[i];
if (b.BodyType == BodyType.Static)
{
b.Flags &= ~BodyFlags.Island;
}
}
}
// Synchronize fixtures, check for out of range bodies.
foreach (Body b in BodyList)
{
// If a body was not in an island then it did not move.
if ((b.Flags & BodyFlags.Island) != BodyFlags.Island)
{
continue;
}
if (b.BodyType == BodyType.Static)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
ContactManager.FindNewContacts();
}
/// <summary>
/// Find TOI contacts and solve them.
/// </summary>
/// <param name="step">The step.</param>
private void SolveTOI(ref TimeStep step)
{
Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager);
if (_stepComplete)
{
for (int i = 0; i < BodyList.Count; i++)
{
BodyList[i].Flags &= ~BodyFlags.Island;
BodyList[i].Sweep.Alpha0 = 0.0f;
}
for (int i = 0; i < ContactManager.ContactList.Count; i++)
{
Contact c = ContactManager.ContactList[i];
// Invalidate TOI
c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
c.TOICount = 0;
c.TOI = 1.0f;
}
}
// Find TOI events and solve them.
for (; ; )
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
for (int i = 0; i < ContactManager.ContactList.Count; i++)
{
Contact c = ContactManager.ContactList[i];
// Is this contact disabled?
if (c.Enabled == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.TOICount > Settings.MaxSubSteps)
{
continue;
}
float alpha;
if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI)
{
// This contact has a valid cached TOI.
alpha = c.TOI;
}
else
{
Fixture fA = c.FixtureA;
Fixture fB = c.FixtureB;
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
Body bA = fA.Body;
Body bB = fB.Body;
BodyType typeA = bA.BodyType;
BodyType typeB = bB.BodyType;
Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic);
bool awakeA = bA.Awake && typeA != BodyType.Static;
bool awakeB = bB.Awake && typeB != BodyType.Static;
// Is at least one body awake?
if (awakeA == false && awakeB == false)
{
continue;
}
bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && !bA.IgnoreCCD;
bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.Sweep.Alpha0;
if (bA.Sweep.Alpha0 < bB.Sweep.Alpha0)
{
alpha0 = bB.Sweep.Alpha0;
bA.Sweep.Advance(alpha0);
}
else if (bB.Sweep.Alpha0 < bA.Sweep.Alpha0)
{
alpha0 = bA.Sweep.Alpha0;
bB.Sweep.Advance(alpha0);
}
Debug.Assert(alpha0 < 1.0f);
// Compute the time of impact in interval [0, minTOI]
_input.ProxyA.Set(fA.Shape, c.ChildIndexA);
_input.ProxyB.Set(fB.Shape, c.ChildIndexB);
_input.SweepA = bA.Sweep;
_input.SweepB = bB.Sweep;
_input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, _input);
// Beta is the fraction of the remaining portion of the .
float beta = output.T;
if (output.State == TOIOutputState.Touching)
{
alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.TOI = alpha;
c.Flags |= ContactFlags.TOI;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha)
{
// No more TOI events. Done!
_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
Fixture fA1 = minContact.FixtureA;
Fixture fB1 = minContact.FixtureB;
Body bA1 = fA1.Body;
Body bB1 = fB1.Body;
Sweep backup1 = bA1.Sweep;
Sweep backup2 = bB1.Sweep;
bA1.Advance(minAlpha);
bB1.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(ContactManager);
minContact.Flags &= ~ContactFlags.TOI;
++minContact.TOICount;
// Is the contact solid?
if (minContact.Enabled == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.Enabled = false;
bA1.Sweep = backup1;
bB1.Sweep = backup2;
bA1.SynchronizeTransform();
bB1.SynchronizeTransform();
continue;
}
bA1.Awake = true;
bB1.Awake = true;
// Build the island
Island.Clear();
Island.Add(bA1);
Island.Add(bB1);
Island.Add(minContact);
bA1.Flags |= BodyFlags.Island;
bB1.Flags |= BodyFlags.Island;
minContact.Flags |= ContactFlags.Island;
// Get contacts on bodyA and bodyB.
Body[] bodies = { bA1, bB1 };
for (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.BodyType == BodyType.Dynamic)
{
// for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next)
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
// Has this contact already been added to the island?
if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
Body other = ce.Other;
if (other.BodyType == BodyType.Dynamic &&
body.IsBullet == false && other.IsBullet == false)
{
continue;
}
// Skip sensors.
if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor)
{
continue;
}
// Tentatively advance the body to the TOI.
Sweep backup = other.Sweep;
if ((other.Flags & BodyFlags.Island) == 0)
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(ContactManager);
// Was the contact disabled by the user?
if (contact.Enabled == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact.IsTouching() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Add the contact to the island
contact.Flags |= ContactFlags.Island;
Island.Add(contact);
// Has the other body already been added to the island?
if ((other.Flags & BodyFlags.Island) == BodyFlags.Island)
{
continue;
}
// Add the other body to the island.
other.Flags |= BodyFlags.Island;
if (other.BodyType != BodyType.Static)
{
other.Awake = true;
}
Island.Add(other);
}
}
}
TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
//subStep.positionIterations = 20;
//subStep.velocityIterations = step.velocityIterations;
//subStep.warmStarting = false;
Island.SolveTOI(ref subStep);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < Island.BodyCount; ++i)
{
Body body = Island.Bodies[i];
body.Flags &= ~BodyFlags.Island;
if (body.BodyType != BodyType.Dynamic)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
ContactManager.FindNewContacts();
if (EnableSubStepping)
{
_stepComplete = false;
break;
}
}
}
public void AddController(Controller controller)
{
Debug.Assert(!ControllerList.Contains(controller), "You are adding the same controller more than once.");
controller.World = this;
ControllerList.Add(controller);
if (ControllerAdded != null)
ControllerAdded(controller);
}
public void RemoveController(Controller controller)
{
Debug.Assert(ControllerList.Contains(controller),
"You are removing a controller that is not in the simulation.");
if (ControllerList.Contains(controller))
{
ControllerList.Remove(controller);
if (ControllerRemoved != null)
ControllerRemoved(controller);
}
}
public void AddBreakableBody(BreakableBody breakableBody)
{
BreakableBodyList.Add(breakableBody);
}
public void RemoveBreakableBody(BreakableBody breakableBody)
{
//The breakable body list does not contain the body you tried to remove.
Debug.Assert(BreakableBodyList.Contains(breakableBody));
BreakableBodyList.Remove(breakableBody);
}
public Fixture TestPoint(Vector2 point)
{
AABB aabb;
Vector2 d = new Vector2(Settings.Epsilon, Settings.Epsilon);
aabb.LowerBound = point - d;
aabb.UpperBound = point + d;
Fixture myFixture = null;
// Query the world for overlapping shapes.
QueryAABB(
fixture =>
{
bool inside = fixture.TestPoint(ref point);
if (inside)
{
myFixture = fixture;
return false;
}
// Continue the query.
return true;
}, ref aabb);
return myFixture;
}
/// <summary>
/// Returns a list of fixtures that are at the specified point.
/// </summary>
/// <param name="point">The point.</param>
/// <returns></returns>
public List<Fixture> TestPointAll(Vector2 point)
{
AABB aabb;
Vector2 d = new Vector2(Settings.Epsilon, Settings.Epsilon);
aabb.LowerBound = point - d;
aabb.UpperBound = point + d;
List<Fixture> fixtures = new List<Fixture>();
// Query the world for overlapping shapes.
QueryAABB(
fixture =>
{
bool inside = fixture.TestPoint(ref point);
if (inside)
fixtures.Add(fixture);
// Continue the query.
return true;
}, ref aabb);
return fixtures;
}
public void Clear()
{
ProcessChanges();
for (int i = BodyList.Count - 1; i >= 0; i--)
{
RemoveBody(BodyList[i]);
}
for (int i = ControllerList.Count - 1; i >= 0; i--)
{
RemoveController(ControllerList[i]);
}
for (int i = BreakableBodyList.Count - 1; i >= 0; i--)
{
RemoveBreakableBody(BreakableBodyList[i]);
}
ProcessChanges();
}
}
}