/// <summary>
/// Destroy a fixture. This removes the fixture from the broad-phase and
/// destroys all contacts associated with this fixture. This will
/// automatically adjust the mass of the body if the body is dynamic and the
/// fixture has positive density.
/// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
/// Warning: This function is locked during callbacks.
/// </summary>
/// <param name="fixture">The fixture to be removed.</param>
public void DestroyFixture(Fixture fixture)
{
Debug.Assert(fixture.Body == this);
// Remove the fixture from this body's singly linked list.
Debug.Assert(FixtureList.Count > 0);
// You tried to remove a fixture that not present in the fixturelist.
Debug.Assert(FixtureList.Contains(fixture));
// Destroy any contacts associated with the fixture.
ContactEdge edge = ContactList;
while (edge != null)
{
Contact c = edge.Contact;
edge = edge.Next;
Fixture fixtureA = c.FixtureA;
Fixture fixtureB = c.FixtureB;
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
World.ContactManager.Destroy(c);
}
}
if ((Flags & BodyFlags.Enabled) == BodyFlags.Enabled)
{
IBroadPhase broadPhase = World.ContactManager.BroadPhase;
fixture.DestroyProxies(broadPhase);
}
FixtureList.Remove(fixture);
fixture.Destroy();
fixture.Body = null;
ResetMassData();
}
/// <summary>
/// Destroy a fixture. This removes the fixture from the broad-phase and
/// destroys all contacts associated with this fixture. This will
/// automatically adjust the mass of the body if the body is dynamic and the
/// fixture has positive density.
/// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
/// Warning: This function is locked during callbacks.
/// </summary>
/// <param name="fixture">The fixture to be removed.</param>
public void destroyFixture(Fixture fixture)
{
Debug.Assert(fixture.body == this);
// Remove the fixture from this body's singly linked list.
Debug.Assert(fixtureList.Count > 0);
// You tried to remove a fixture that not present in the fixturelist.
Debug.Assert(fixtureList.Contains(fixture));
// Destroy any contacts associated with the fixture.
ContactEdge edge = contactList;
while (edge != null)
{
var c = edge.contact;
edge = edge.next;
var fixtureA = c.fixtureA;
var fixtureB = c.fixtureB;
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
_world.contactManager.destroy(c);
}
}
if (_enabled)
{
var broadPhase = _world.contactManager.broadPhase;
fixture.destroyProxies(broadPhase);
}
fixtureList.Remove(fixture);
fixture.destroy();
fixture.body = null;
resetMassData();
}
public ContactEdge Restore(bool restoreLinks, Dictionary <string, TrueSync.Physics2D.Contact> contactDic, Dictionary <string, ContactEdge> contactEdgeDic)
{
string key = this.contactKey + "_" + this.body.BodyId;
ContactEdge result;
if (restoreLinks)
{
ContactEdge contactEdge = contactEdgeDic[key];
bool flag = this.nextEdge != null;
if (flag)
{
contactEdge.Next = contactEdgeDic[this.nextEdge];
}
bool flag2 = this.previousEdge != null;
if (flag2)
{
contactEdge.Prev = contactEdgeDic[this.previousEdge];
}
result = contactEdge;
}
else
{
bool flag3 = contactEdgeDic.ContainsKey(key);
if (flag3)
{
result = contactEdgeDic[key];
}
else
{
ContactEdge @new = WorldClone2D.poolContactEdge.GetNew();
@new.Contact = contactDic[this.contactKey];
@new.Other = this.body;
contactEdgeDic[key] = @new;
result = @new;
}
}
return(result);
}
private bool AreBodiesColliding(Body bodyA_, Body bodyB_)
{
// If either of the bodies are null
if (bodyA_ == null || bodyB_ == null)
{
return(false);
}
// If either of the bodies have no contact list
if (bodyA_.ContactList == null || bodyB_.ContactList == null)
{
return(false);
}
// Contact point for the body
ContactEdge ce = bodyA_.ContactList;
// While there is a valid contact point
while (ce != null)
{
// If the two bodies are colliding (AABB)
if (ce.Other == bodyB_)
{
Contact c = ce.Contact;
// If the two bodies are physically touching
if (c.IsTouching && c.Enabled)
{
return(true);
}
}
// Get next contact point
ce = ce.Next;
}
// If the two bodies are not colliding
return(false);
}
/// <summary>
/// Call this if you want to establish collision that was previously disabled by
/// ContactFilter::ShouldCollide.
/// </summary>
public void Refilter()
{
if (Body == null)
{
return;
}
// Flag associated contacts for filtering.
ContactEdge edge = Body.ContactList;
while (edge != null)
{
Contact contact = edge.Contact;
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
if (fixtureA == this || fixtureB == this)
{
contact.SetFlagForFiltering();
}
edge = edge.Next;
}
World world = Body.World;
if (world == null)
{
return;
}
// Touch each proxy so that new pairs may be created
BroadPhase broadPhase = world.ContactManager.BroadPhase;
for (int i = 0; i < ProxyCount; ++i)
{
broadPhase.TouchProxy(Proxies[i].ProxyId);
}
}
/**
* Call this if you want to establish collision that was previously disabled by
* ContactFilter::ShouldCollide.
*/
public void refilter()
{
if (m_body == null)
{
return;
}
// Flag associated contacts for filtering.
ContactEdge edge = m_body.getContactList();
while (edge != null)
{
Contact contact = edge.contact;
Fixture fixtureA = contact.getFixtureA();
Fixture fixtureB = contact.getFixtureB();
if (fixtureA == this || fixtureB == this)
{
contact.flagForFiltering();
}
edge = edge.next;
}
World world = m_body.getWorld();
if (world == null)
{
return;
}
// Touch each proxy so that new pairs may be created
BroadPhase broadPhase = world.m_contactManager.m_broadPhase;
for (int i = 0; i < m_proxyCount; ++i)
{
broadPhase.touchProxy(m_proxies[i].proxyId);
}
}
public void AreaAttack()
{
ContactEdge c = _body.ContactList;
while (c != null && c.Next != null)
{
if (c.Contact.IsTouching)
{
Entity e = (Entity)c.Other.UserData;
if (e.GetComponent <CircleCollider>() != null)
{
Vector2 force = e.Position - _owner.Position;
force.Normalize();
force *= m_areaKnockback;
e.GetComponent <CircleCollider>().ApplyForce(force, _owner.Position);
}
if (e.GetComponent <Damageable>() != null)
{
e.GetComponent <Damageable>().DealDamage(m_areaAttack);
}
}
c = c.Next;
}
}
/// <summary>
/// Destroy a joint. This may cause the connected bodies to begin colliding.
/// Warning: This function is locked during callbacks.
/// </summary>
/// <param name="joint">The joint.</param>
public void RemoveJoint(Joint joint)
{
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
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);
}
}
/**
* Destroy a fixture. This removes the fixture from the broad-phase and destroys all contacts
* associated with this fixture. This will automatically adjust the mass of the body if the body
* is dynamic and the fixture has positive density. All fixtures attached to a body are implicitly
* destroyed when the body is destroyed.
*
* @param fixture the fixture to be removed.
* @warning This function is locked during callbacks.
*/
public void destroyFixture(Fixture fixture)
{
if (m_world.isLocked())
{
return;
}
// Remove the fixture from this body's singly linked list.
Fixture node = m_fixtureList;
Fixture last = null; // java change
bool found = false;
while (node != null)
{
if (node == fixture)
{
node = fixture.m_next;
found = true;
break;
}
last = node;
node = node.m_next;
}
// You tried to remove a shape that is not attached to this body.
// java change, remove it from the list
if (last == null)
{
m_fixtureList = fixture.m_next;
}
else
{
last.m_next = fixture.m_next;
}
// Destroy any contacts associated with the fixture.
ContactEdge edge = m_contactList;
while (edge != null)
{
Contact c = edge.contact;
edge = edge.next;
Fixture fixtureA = c.getFixtureA();
Fixture fixtureB = c.getFixtureB();
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
m_world.m_contactManager.destroy(c);
}
}
if ((m_flags & e_activeFlag) == e_activeFlag)
{
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
fixture.destroyProxies(broadPhase);
}
fixture.destroy();
fixture.m_body = null;
fixture.m_next = null;
fixture = null;
--m_fixtureCount;
// Reset the mass data.
resetMassData();
}
public Body(BodyDef bd, World world)
{
m_flags = 0;
if (bd.bullet)
{
m_flags |= e_bulletFlag;
}
if (bd.fixedRotation)
{
m_flags |= e_fixedRotationFlag;
}
if (bd.allowSleep)
{
m_flags |= e_autoSleepFlag;
}
if (bd.awake)
{
m_flags |= e_awakeFlag;
}
if (bd.active)
{
m_flags |= e_activeFlag;
}
m_world = world;
m_xf.p.set(bd.position);
m_xf.q.set(bd.angle);
m_sweep.localCenter.setZero();
m_sweep.c0.set(m_xf.p);
m_sweep.c.set(m_xf.p);
m_sweep.a0 = bd.angle;
m_sweep.a = bd.angle;
m_sweep.alpha0 = 0.0d;
m_jointList = null;
m_contactList = null;
m_prev = null;
m_next = null;
m_linearVelocity.set(bd.linearVelocity);
m_angularVelocity = bd.angularVelocity;
m_linearDamping = bd.linearDamping;
m_angularDamping = bd.angularDamping;
m_gravityScale = bd.gravityScale;
m_force.setZero();
m_torque = 0.0d;
m_sleepTime = 0.0d;
m_type = bd.type;
if (m_type == BodyType.DYNAMIC)
{
m_mass = 1d;
m_invMass = 1d;
}
else
{
m_mass = 0d;
m_invMass = 0d;
}
m_I = 0.0d;
m_invI = 0.0d;
m_userData = bd.userData;
m_fixtureList = null;
m_fixtureCount = 0;
}
private void HandleLimbCollision(Contact contact, Limb limb)
{
if (limb.Mass > 100.0f)
{
Vector2 normal = Vector2.Normalize(Body.SimPosition - limb.SimPosition);
float impact = Math.Min(Vector2.Dot(Velocity - limb.LinearVelocity, -normal), 50.0f) / 5.0f * Math.Min(limb.Mass / 200.0f, 1);
ApplyImpact(impact, -normal, contact);
foreach (Submarine dockedSub in submarine.DockedTo)
{
dockedSub.SubBody.ApplyImpact(impact, -normal, contact);
}
}
//find all contacts between the limb and level walls
List <Contact> levelContacts = new List <Contact>();
ContactEdge contactEdge = limb.body.FarseerBody.ContactList;
while (contactEdge.Next != null)
{
if (contactEdge.Contact.Enabled &&
contactEdge.Other.UserData is VoronoiCell &&
contactEdge.Contact.IsTouching)
{
levelContacts.Add(contactEdge.Contact);
}
contactEdge = contactEdge.Next;
}
if (levelContacts.Count == 0)
{
return;
}
//if the limb is in contact with the level, apply an artifical impact to prevent the sub from bouncing on top of it
//not a very realistic way to handle the collisions (makes it seem as if the characters were made of reinforced concrete),
//but more realistic than bouncing and prevents using characters as "bumpers" that prevent all collision damage
//TODO: apply impact damage and/or gib the character that got crushed between the sub and the level?
Vector2 avgContactNormal = Vector2.Zero;
foreach (Contact levelContact in levelContacts)
{
Vector2 contactNormal;
FixedArray2 <Vector2> temp;
levelContact.GetWorldManifold(out contactNormal, out temp);
//if the contact normal is pointing from the limb towards the level cell it's touching, flip the normal
VoronoiCell cell = levelContact.FixtureB.UserData is VoronoiCell ?
((VoronoiCell)levelContact.FixtureB.UserData) : ((VoronoiCell)levelContact.FixtureA.UserData);
var cellDiff = ConvertUnits.ToDisplayUnits(limb.body.SimPosition) - cell.Center;
if (Vector2.Dot(contactNormal, cellDiff) < 0)
{
contactNormal = -contactNormal;
}
avgContactNormal += contactNormal;
//apply impacts at the positions where this sub is touching the limb
ApplyImpact((Vector2.Dot(-Velocity, contactNormal) / 2.0f) / levelContacts.Count, contactNormal, levelContact);
}
avgContactNormal /= levelContacts.Count;
float contactDot = Vector2.Dot(Body.LinearVelocity, -avgContactNormal);
if (contactDot > 0.0f)
{
Body.LinearVelocity -= Vector2.Normalize(Body.LinearVelocity) * contactDot;
float damageAmount = contactDot * Body.Mass / limb.character.Mass;
Vector2 n;
FixedArray2 <Vector2> contactPos;
contact.GetWorldManifold(out n, out contactPos);
limb.character.DamageLimb(ConvertUnits.ToDisplayUnits(contactPos[0]), limb, DamageType.Blunt, damageAmount, 0.0f, 0.0f, true, 0.0f);
if (limb.character.IsDead)
{
foreach (LimbJoint limbJoint in limb.character.AnimController.LimbJoints)
{
if (limbJoint.IsSevered || (limbJoint.LimbA != limb && limbJoint.LimbB != limb))
{
continue;
}
limb.character.AnimController.SeverLimbJoint(limbJoint);
}
}
}
}
// Broad-phase callback.
private void AddPair(int proxyIdA, int proxyIdB)
{
FixtureProxy proxyA = BroadPhase.GetProxy(proxyIdA);
FixtureProxy proxyB = BroadPhase.GetProxy(proxyIdB);
Fixture fixtureA = proxyA.Fixture;
Fixture fixtureB = proxyB.Fixture;
int indexA = proxyA.ChildIndex;
int indexB = proxyB.ChildIndex;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// Does a contact already exist?
for (ContactEdge ceB = bodyB.ContactList; ceB != null; ceB = ceB.Next)
{
if (ceB.Other == bodyA)
{
Fixture fA = ceB.Contact.FixtureA;
Fixture fB = ceB.Contact.FixtureB;
int iA = ceB.Contact.ChildIndexA;
int iB = ceB.Contact.ChildIndexB;
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
//Check default filter
if (ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Check user filtering.
var contactFilterHandler = ContactFilter;
if (contactFilterHandler != null)
{
if (contactFilterHandler(fixtureA, fixtureB) == false)
{
return;
}
}
//FPE feature: BeforeCollision delegate
var beforeCollisionHandlerA = fixtureA.BeforeCollision;
if (beforeCollisionHandlerA != null)
{
if (beforeCollisionHandlerA(fixtureA, fixtureB) == false)
{
return;
}
}
var beforeCollisionHandlerB = fixtureB.BeforeCollision;
if (beforeCollisionHandlerB != null)
{
if (beforeCollisionHandlerB(fixtureB, fixtureA) == false)
{
return;
}
}
// Call the factory.
Contact c = Contact.Create(this, fixtureA, indexA, fixtureB, indexB);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.FixtureA;
fixtureB = c.FixtureB;
bodyA = fixtureA.Body;
bodyB = fixtureB.Body;
// Insert into the world.
c.Prev = ContactList;
c.Next = c.Prev.Next;
c.Prev.Next = c;
c.Next.Prev = c;
ContactCount++;
#if USE_ACTIVE_CONTACT_SET
ActiveContacts.Add(c);
#endif
// Connect to island graph.
// Connect to body A
c._nodeA.Contact = c;
c._nodeA.Other = bodyB;
c._nodeA.Prev = null;
c._nodeA.Next = bodyA.ContactList;
if (bodyA.ContactList != null)
{
bodyA.ContactList.Prev = c._nodeA;
}
bodyA.ContactList = c._nodeA;
// Connect to body B
c._nodeB.Contact = c;
c._nodeB.Other = bodyA;
c._nodeB.Prev = null;
c._nodeB.Next = bodyB.ContactList;
if (bodyB.ContactList != null)
{
bodyB.ContactList.Prev = c._nodeB;
}
bodyB.ContactList = c._nodeB;
// Wake up the bodies
if (fixtureA.IsSensor == false && fixtureB.IsSensor == false)
{
bodyA.Awake = true;
bodyB.Awake = true;
}
}
private void HandleLimbCollision(Impact collision, Limb limb)
{
if (limb?.body?.FarseerBody == null || limb.character == null)
{
return;
}
if (limb.Mass > MinImpactLimbMass)
{
Vector2 normal =
Vector2.DistanceSquared(Body.SimPosition, limb.SimPosition) < 0.0001f ?
Vector2.UnitY :
Vector2.Normalize(Body.SimPosition - limb.SimPosition);
float impact = Math.Min(Vector2.Dot(collision.Velocity, -normal), 50.0f) * Math.Min(limb.Mass / 100.0f, 1);
ApplyImpact(impact, -normal, collision.ImpactPos, applyDamage: false);
foreach (Submarine dockedSub in submarine.DockedTo)
{
dockedSub.SubBody.ApplyImpact(impact, -normal, collision.ImpactPos, applyDamage: false);
}
}
//find all contacts between the limb and level walls
List <Contact> levelContacts = new List <Contact>();
ContactEdge contactEdge = limb.body.FarseerBody.ContactList;
while (contactEdge?.Contact != null)
{
if (contactEdge.Contact.Enabled &&
contactEdge.Contact.IsTouching &&
contactEdge.Other?.UserData is VoronoiCell)
{
levelContacts.Add(contactEdge.Contact);
}
contactEdge = contactEdge.Next;
}
if (levelContacts.Count == 0)
{
return;
}
//if the limb is in contact with the level, apply an artifical impact to prevent the sub from bouncing on top of it
//not a very realistic way to handle the collisions (makes it seem as if the characters were made of reinforced concrete),
//but more realistic than bouncing and prevents using characters as "bumpers" that prevent all collision damage
Vector2 avgContactNormal = Vector2.Zero;
foreach (Contact levelContact in levelContacts)
{
levelContact.GetWorldManifold(out Vector2 contactNormal, out FixedArray2 <Vector2> temp);
//if the contact normal is pointing from the limb towards the level cell it's touching, flip the normal
VoronoiCell cell = levelContact.FixtureB.UserData is VoronoiCell ?
((VoronoiCell)levelContact.FixtureB.UserData) : ((VoronoiCell)levelContact.FixtureA.UserData);
var cellDiff = ConvertUnits.ToDisplayUnits(limb.body.SimPosition) - cell.Center;
if (Vector2.Dot(contactNormal, cellDiff) < 0)
{
contactNormal = -contactNormal;
}
avgContactNormal += contactNormal;
//apply impacts at the positions where this sub is touching the limb
ApplyImpact((Vector2.Dot(-collision.Velocity, contactNormal) / 2.0f) / levelContacts.Count, contactNormal, collision.ImpactPos, applyDamage: false);
}
avgContactNormal /= levelContacts.Count;
float contactDot = Vector2.Dot(Body.LinearVelocity, -avgContactNormal);
if (contactDot > 0.001f)
{
Vector2 velChange = Vector2.Normalize(Body.LinearVelocity) * contactDot;
if (!MathUtils.IsValid(velChange))
{
GameAnalyticsManager.AddErrorEventOnce(
"SubmarineBody.HandleLimbCollision:" + submarine.ID,
GameAnalyticsSDK.Net.EGAErrorSeverity.Error,
"Invalid velocity change in SubmarineBody.HandleLimbCollision (submarine velocity: " + Body.LinearVelocity
+ ", avgContactNormal: " + avgContactNormal
+ ", contactDot: " + contactDot
+ ", velChange: " + velChange + ")");
return;
}
Body.LinearVelocity -= velChange;
float damageAmount = contactDot * Body.Mass / limb.character.Mass;
limb.character.LastDamageSource = submarine;
limb.character.DamageLimb(ConvertUnits.ToDisplayUnits(collision.ImpactPos), limb,
AfflictionPrefab.ImpactDamage.Instantiate(damageAmount).ToEnumerable(), 0.0f, true, 0.0f);
if (limb.character.IsDead)
{
foreach (LimbJoint limbJoint in limb.character.AnimController.LimbJoints)
{
if (limbJoint.IsSevered || (limbJoint.LimbA != limb && limbJoint.LimbB != limb))
{
continue;
}
limb.character.AnimController.SeverLimbJoint(limbJoint);
}
}
}
}
/// <summary>
/// This is a high-level function to cuts fixtures inside the given world, using the start and end points.
/// Note: We don't support cutting when the start or end is inside a shape.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="start">The startpoint.</param>
/// <param name="end">The endpoint.</param>
/// <param name="thickness">The thickness of the cut</param>
public static Vector2 Cut(World world, Vector2 start, Vector2 end, float thickness, Category collisionCategories = Category.None)
{
// The left side of the cut will remain part of the existing body;
// the right side will be made into a new body
List <Fixture> fixtures = new List <Fixture>();
List <Vector2> entryPoints = new List <Vector2>();
List <Vector2> exitPoints = new List <Vector2>();
List <RayCastResult> results = new List <RayCastResult>();
//float blockingFraction = float.MaxValue;
Vector2 stoppingPoint = end;
//We don't support cutting when the start or end is inside a shape.
//if (world.TestPoint(start) != null || world.TestPoint(end) != null)
// return;
//Get the entry points
world.RayCast((f, p, n, fr) =>
{
RayCastResult r = new RayCastResult();
r.f = f;
r.p = p;
r.fr = fr;
results.Add(r);
return(1);
}, start, end);
results = results.OrderBy(p => p.fr).ToList();
foreach (RayCastResult r in results)
{
if ((r.f.CollisionCategories & collisionCategories) != Category.None)
{
stoppingPoint = r.p;
break;
}
if (!r.f.TestPoint(ref end))
{
if (world.FixtureCut != null)
{
world.FixtureCut(r.f);
}
fixtures.Add(r.f);
entryPoints.Add(r.p);
}
}
//Reverse the ray to get the exitpoints
world.RayCast((f, p, n, fr) =>
{
if (fixtures.Contains(f))
{
exitPoints.Add(p);
}
return(1);
}, end, start);
Debug.Assert(entryPoints.Count == exitPoints.Count && entryPoints.Count == fixtures.Count);
//Fixture containsEnd = world.TestPoint(end);
//if (containsEnd != null)
//{
// entryPoints.RemoveAt(0);
// fixtures.Remove(containsEnd);
//}
//Fixture containsStart = world.TestPoint(start);
//if (containsStart != null)
//{
// exitPoints.RemoveAt(exitPoints.Count - 1);
// fixtures.Remove(containsStart);
//}
//We only have a single point. We need at least 2
if (entryPoints.Count + exitPoints.Count < 2)
{
return(stoppingPoint);
}
var query =
(from fix in fixtures
select fix.Body).Distinct();
foreach (Body b in query)
{
if (b == null || b.BodyType == BodyType.Static)
{
continue;
}
ContactEdge edge = b.ContactList;
while (edge != null)
{
Contact c = edge.Contact;
edge = edge.Next;
world.ContactManager.Destroy(c);
}
List <Body> leftBodies = new List <Body>();
List <Body> rightBodies = new List <Body>();
//Body rightBody = new Body(world);
List <Joint> leftJoints = new List <Joint>();
List <Joint> rightJoints = new List <Joint>();
foreach (Joint j in b.JointList)
{
if (isLeft(start, end, j.WorldAnchorA))
{
leftJoints.Add(j);
}
else
{
rightJoints.Add(j);
}
}
//List<Fixture> leftList = new List<Fixture>();
//List<Fixture> rightList = new List<Fixture>();
Fixture[] bodyFixtures = new Fixture[b.FixtureList.Count];
b.FixtureList.CopyTo(bodyFixtures);
b.FixtureList.Clear();
//leftBodies.Add(b);
// For each fixture that was sliced through...
foreach (Fixture fix in (from f in bodyFixtures where fixtures.Contains(f) select f))
{
int i = fixtures.IndexOf(fix);
// split this in half and put the halves in the over/under lists
Vertices first;
Vertices second;
SplitShape(fix, entryPoints[i], exitPoints[i], thickness, out first, out second);
if (!SanityCheck(first) || !SanityCheck(second))
{
continue;
}
PolygonShape leftShape = new PolygonShape(first, fix.Shape.Density);
PolygonShape rightShape = new PolygonShape(second, fix.Shape.Density);
if (!b.FixtureList.Any())
{
if (leftShape.MassData.Area > rightShape.MassData.Area)
{
b.CreateFixture(leftShape, fix.UserData);
leftBodies.Add(b);
GlomFixture(world, b, rightBodies, rightShape, fix.UserData, rightJoints);
}
else
{
b.CreateFixture(rightShape, fix.UserData);
rightBodies.Add(b);
GlomFixture(world, b, leftBodies, leftShape, fix.UserData, leftJoints);
}
}
else
{
GlomFixture(world, b, leftBodies, leftShape, fix.UserData, leftJoints);
GlomFixture(world, b, rightBodies, rightShape, fix.UserData, rightJoints);
}
}
// for each fixture that was NOT sliced through...
foreach (Fixture fix in (from f in bodyFixtures where !fixtures.Contains(f) select f))
{
if (isLeft(start, end, fix))
{
GlomFixture(world, b, leftBodies, fix.Shape, fix.UserData, leftJoints);
}
else
{
GlomFixture(world, b, rightBodies, fix.Shape, fix.UserData, rightJoints);
//rightBody.CreateFixture(fix.Shape.Clone(), fix.UserData);
}
}
foreach (Body bod in leftBodies.Concat(rightBodies))
{
bod.ResetMassData();
bod.BodyType = BodyType.Dynamic;
bod.Rotation = b.Rotation;
bod.LinearVelocity = b.LinearVelocity;
bod.AngularVelocity = b.AngularVelocity;
bod.Position = b.Position;
}
//b.JointList = null;
//world.RemoveBody(b);
foreach (Fixture f in bodyFixtures)
{
b.DestroyFixture(f);
}
world.ProcessChanges();
}
return(stoppingPoint);
}
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.IslandFlag;
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.IslandFlag;
c._flags &= ~ContactFlags.TOIFlag;
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.TOIFlag)
{
// 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 activeA = bA.Awake && typeA != BodyType.Static;
bool activeB = bB.Awake && typeB != BodyType.Static;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && (fA.IgnoreCCDWith & fB.CollisionCategories) == 0 && !bA.IgnoreCCD;
bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && (fB.IgnoreCCDWith & fA.CollisionCategories) == 0 && !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]
TOIInput input = new TOIInput();
input.ProxyA = new DistanceProxy(fA.Shape, c.ChildIndexA);
input.ProxyB = new DistanceProxy(fB.Shape, c.ChildIndexB);
input.SweepA = bA._sweep;
input.SweepB = bB._sweep;
input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(ref input, out output);
// 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.TOIFlag;
}
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 bA0 = fA1.Body;
Body bB0 = fB1.Body;
Sweep backup1 = bA0._sweep;
Sweep backup2 = bB0._sweep;
bA0.Advance(minAlpha);
bB0.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(ContactManager);
minContact._flags &= ~ContactFlags.TOIFlag;
++minContact._toiCount;
// Is the contact solid?
if (minContact.Enabled == false || minContact.IsTouching == false)
{
// Restore the sweeps.
minContact._flags &= ~ContactFlags.EnabledFlag;
bA0._sweep = backup1;
bB0._sweep = backup2;
bA0.SynchronizeTransform();
bB0.SynchronizeTransform();
continue;
}
bA0.Awake = true;
bB0.Awake = true;
// Build the island
Island.Clear();
Island.Add(bA0);
Island.Add(bB0);
Island.Add(minContact);
bA0._flags |= BodyFlags.IslandFlag;
bB0._flags |= BodyFlags.IslandFlag;
minContact._flags &= ~ContactFlags.IslandFlag;
// Get contacts on bodyA and bodyB.
Body[] bodies = { bA0, bB0 };
for (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.BodyType == BodyType.Dynamic)
{
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
if (Island.BodyCount == Island.BodyCapacity)
{
break;
}
if (Island.ContactCount == Island.ContactCapacity)
{
break;
}
// Has this contact already been added to the island?
if (contact.IslandFlag)
{
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.IsIsland)
{
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
minContact._flags |= ContactFlags.IslandFlag;
Island.Add(contact);
// Has the other body already been added to the island?
if (other.IsIsland)
{
continue;
}
// Add the other body to the island.
other._flags |= BodyFlags.IslandFlag;
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;
Island.SolveTOI(ref subStep, bA0.IslandIndex, bB0.IslandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < Island.BodyCount; ++i)
{
Body body = Island.Bodies[i];
body._flags &= ~BodyFlags.IslandFlag;
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.TOIFlag;
ce.Contact._flags &= ~ContactFlags.IslandFlag;
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
ContactManager.FindNewContacts();
if (Settings.EnableSubStepping)
{
_stepComplete = false;
break;
}
}
}
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._flags |= ContactFlags.FilterFlag;
}
edge = edge.Next;
}
}
}
JointAdded?.Invoke(joint);
// Note: creating a joint doesn't wake the bodies.
}
_jointAddList.Clear();
}
}
private void AddPair(object proxyUserDataA, object proxyUserDataB)
{
var proxyA = (FixtureProxy)proxyUserDataA;
var proxyB = (FixtureProxy)proxyUserDataB;
Fixture fixtureA = proxyA.fixture;
Fixture fixtureB = proxyB.fixture;
int indexA = proxyA.childIndex;
int indexB = proxyB.childIndex;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// TODO_ERIN use a hash table to remove a potential bottleneck when both
// bodies have a lot of contacts.
// Does a contact already exist?
ContactEdge edge = bodyB.GetContactList();
while (edge != null)
{
if (edge.other == bodyA)
{
Fixture fA = edge.contact.GetFixtureA();
Fixture fB = edge.contact.GetFixtureB();
int iA = edge.contact.GetChildIndexA();
int iB = edge.contact.GetChildIndexB();
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
// A contact already exists.
{
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
// A contact already exists.
{
return;
}
}
edge = edge.next;
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if (m_contactFilter != null && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
var c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.GetFixtureA();
fixtureB = c.GetFixtureB();
indexA = c.GetChildIndexA();
indexB = c.GetChildIndexB();
bodyA = fixtureA.GetBody();
bodyB = fixtureB.GetBody();
// Insert into the world.
c.m_prev = null;
c.m_next = m_contactList;
if (m_contactList != null)
{
m_contactList.m_prev = c;
}
m_contactList = c;
// Connect to island graph.
// Connect to body A
c.m_nodeA.contact = c;
c.m_nodeA.other = bodyB;
c.m_nodeA.prev = null;
c.m_nodeA.next = bodyA.m_contactList;
if (bodyA.m_contactList != null)
{
bodyA.m_contactList.prev = c.m_nodeA;
}
bodyA.m_contactList = c.m_nodeA;
// Connect to body B
c.m_nodeB.contact = c;
c.m_nodeB.other = bodyA;
c.m_nodeB.prev = null;
c.m_nodeB.next = bodyB.m_contactList;
if (bodyB.m_contactList != null)
{
bodyB.m_contactList.prev = c.m_nodeB;
}
bodyB.m_contactList = c.m_nodeB;
++m_contactCount;
}
/// <summary>
/// Create a joint to constrain bodies together. This may cause the connected bodies to cease colliding.
/// Warning: This function is locked during callbacks.
/// </summary>
/// <param name="joint">The joint.</param>
public void AddJoint(Joint joint)
{
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
// 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;
}
// WIP David
if (!joint.IsFixedType())
{
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.
}
public void UpdateAttack(float deltaTime, Vector2 attackPosition, IDamageable damageTarget)
{
float dist = ConvertUnits.ToDisplayUnits(Vector2.Distance(SimPosition, attackPosition));
AttackTimer += deltaTime;
body.ApplyTorque(Mass * character.AnimController.Dir * attack.Torque);
bool wasHit = false;
if (damageTarget != null)
{
switch (attack.HitDetectionType)
{
case HitDetection.Distance:
wasHit = dist < attack.DamageRange;
break;
case HitDetection.Contact:
List <Body> targetBodies = new List <Body>();
if (damageTarget is Character)
{
Character targetCharacter = (Character)damageTarget;
foreach (Limb limb in targetCharacter.AnimController.Limbs)
{
if (!limb.IsSevered && limb.body?.FarseerBody != null)
{
targetBodies.Add(limb.body.FarseerBody);
}
}
}
else if (damageTarget is Structure)
{
Structure targetStructure = (Structure)damageTarget;
if (character.Submarine == null && targetStructure.Submarine != null)
{
targetBodies.Add(targetStructure.Submarine.PhysicsBody.FarseerBody);
}
else
{
targetBodies.AddRange(targetStructure.Bodies);
}
}
else if (damageTarget is Item)
{
Item targetItem = damageTarget as Item;
if (targetItem.body?.FarseerBody != null)
{
targetBodies.Add(targetItem.body.FarseerBody);
}
}
if (targetBodies != null)
{
ContactEdge contactEdge = body.FarseerBody.ContactList;
while (contactEdge != null)
{
if (contactEdge.Contact != null &&
contactEdge.Contact.IsTouching &&
targetBodies.Any(b => b == contactEdge.Contact.FixtureA?.Body || b == contactEdge.Contact.FixtureB?.Body))
{
wasHit = true;
break;
}
contactEdge = contactEdge.Next;
}
}
break;
}
}
if (wasHit)
{
if (AttackTimer >= attack.Duration && damageTarget != null)
{
attack.DoDamage(character, damageTarget, WorldPosition, 1.0f, (SoundTimer <= 0.0f));
SoundTimer = SoundInterval;
}
}
Vector2 diff = attackPosition - SimPosition;
if (diff.LengthSquared() < 0.00001f)
{
return;
}
if (attack.ApplyForceOnLimbs != null)
{
foreach (int limbIndex in attack.ApplyForceOnLimbs)
{
if (limbIndex < 0 || limbIndex >= character.AnimController.Limbs.Length)
{
continue;
}
Limb limb = character.AnimController.Limbs[limbIndex];
Vector2 forcePos = limb.pullJoint == null ? limb.body.SimPosition : limb.pullJoint.WorldAnchorA;
limb.body.ApplyLinearImpulse(
limb.Mass * attack.Force * Vector2.Normalize(attackPosition - SimPosition), forcePos);
}
}
else
{
Vector2 forcePos = pullJoint == null ? body.SimPosition : pullJoint.WorldAnchorA;
body.ApplyLinearImpulse(Mass * attack.Force *
Vector2.Normalize(attackPosition - SimPosition), forcePos);
}
}
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;
// 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._flags |= ContactFlags.FilterFlag;
}
edge = edge.Next;
}
}
}
JointRemoved?.Invoke(joint);
}
_jointRemoveList.Clear();
}
}
/// <summary>
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
/// </summary>
/// <param name="body">The body.</param>
private void SolveTOI(Body body)
{
// Find the minimum contact.
Contact toiContact = null;
float toi = 1.0f;
Body toiOther = null;
bool found;
int count;
int iter = 0;
bool bullet = body.IsBullet;
// Iterate until all contacts agree on the minimum TOI. We have
// to iterate because the TOI algorithm may skip some intermediate
// collisions when objects rotate through each other.
do
{
count = 0;
found = false;
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
if (ce.Contact == toiContact)
{
continue;
}
Body other = ce.Other;
BodyType type = other.BodyType;
// Only bullets perform TOI with dynamic bodies.
if (bullet)
{
// Bullets only perform TOI with bodies that have their TOI resolved.
if ((other.Flags & BodyFlags.Toi) == 0)
{
continue;
}
// No repeated hits on non-static bodies
if (type != BodyType.Static && (ce.Contact.Flags & ContactFlags.BulletHit) != 0)
{
continue;
}
}
else if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.Enabled == false)
{
continue;
}
// Prevent infinite looping.
if (contact.TOICount > 10)
{
continue;
}
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
int indexA = contact.ChildIndexA;
int indexB = contact.ChildIndexB;
// Cull sensors.
if (fixtureA.IsSensor || fixtureB.IsSensor)
{
continue;
}
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.ProxyA.Set(fixtureA.Shape, indexA);
input.ProxyB.Set(fixtureB.Shape, indexB);
input.SweepA = bodyA.Sweep;
input.SweepB = bodyB.Sweep;
input.TMax = toi;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
if (output.State == TOIOutputState.Touching && output.T < toi)
{
toiContact = contact;
toi = output.T;
toiOther = other;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == null)
{
body.Advance(1.0f);
return;
}
Sweep backup = body.Sweep;
body.Advance(toi);
toiContact.Update(ContactManager);
if (toiContact.Enabled == false)
{
// Contact disabled. Backup and recurse.
body.Sweep = backup;
SolveTOI(body);
}
++toiContact.TOICount;
// Update all the valid contacts on this body and build a contact island.
count = 0;
for (ContactEdge ce = body.ContactList; (ce != null) && (count < Settings.MaxTOIContacts); ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.BodyType;
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.Enabled == false)
{
continue;
}
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
// Cull sensors.
if (fixtureA.IsSensor || fixtureB.IsSensor)
{
continue;
}
// The contact likely has some new contact points. The listener
// gives the user a chance to disable the contact.
if (contact != toiContact)
{
contact.Update(ContactManager);
}
// Did the user disable the contact?
if (contact.Enabled == false)
{
// Skip this contact.
continue;
}
if (contact.IsTouching() == false)
{
continue;
}
_toiContacts[count] = contact;
++count;
}
// Reduce the TOI body's overlap with the contact island.
_toiSolver.Initialize(_toiContacts, count, body);
const float k_toiBaumgarte = 0.75f;
// bool solved = false;
for (int i = 0; i < 20; ++i)
{
bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte);
if (contactsOkay)
{
// solved = true;
break;
}
}
if (toiOther.BodyType != BodyType.Static)
{
toiContact.Flags |= ContactFlags.BulletHit;
}
}
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.IslandFlag;
}
foreach (Contact c in ContactManager.ContactList)
{
c._flags &= ~ContactFlags.IslandFlag;
}
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.IslandFlag) == BodyFlags.IslandFlag)
{
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.IslandFlag;
// 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 (without resetting sleep timer).
b._flags |= BodyFlags.AwakeFlag;
// 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.IslandFlag)
{
continue;
}
// Is this contact solid and touching?
if (ce.Contact.Enabled == false || ce.Contact.IsTouching == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.FixtureA.IsSensor;
bool sensorB = contact.FixtureB.IsSensor;
if (sensorA || sensorB)
{
continue;
}
Island.Add(contact);
contact._flags |= ContactFlags.IslandFlag;
Body other = ce.Other;
// Was the other body already added to this island?
if (other.IsIsland)
{
continue;
}
Debug.Assert(stackCount < stackSize);
_stack[stackCount++] = other;
other._flags |= BodyFlags.IslandFlag;
}
// 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.IsIsland)
{
continue;
}
Debug.Assert(stackCount < stackSize);
_stack[stackCount++] = other;
other._flags |= BodyFlags.IslandFlag;
}
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.IslandFlag;
}
}
}
// 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.IsIsland)
{
continue;
}
if (b.BodyType == BodyType.Static)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
ContactManager.FindNewContacts();
}
internal XForm _xf; // the body origin transform
#endregion Fields
#region Constructors
internal Body(BodyDef bd, World world)
{
_flags = 0;
if (bd.isBullet)
{
_flags |= BodyFlags.Bullet;
}
if (bd.fixedRotation)
{
_flags |= BodyFlags.FixedRotation;
}
if (bd.allowSleep)
{
_flags |= BodyFlags.AllowSleep;
}
if (bd.isSleeping)
{
_flags |= BodyFlags.Sleep;
}
_world = world;
_xf.Position = bd.position;
_xf.R.Set(bd.angle);
_sweep.localCenter = bd.massData.center;
_sweep.t0 = 1.0f;
_sweep.a0 = _sweep.a = bd.angle;
_sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);
_jointList = null;
_contactList = null;
_prev = null;
_next = null;
_linearVelocity = bd.linearVelocity;
_angularVelocity = bd.angularVelocity;
_linearDamping = bd.linearDamping;
_angularDamping = bd.angularDamping;
_force = new Vector2(0.0f, 0.0f);
_torque = 0.0f;
_linearVelocity = Vector2.Zero;
_angularVelocity = 0.0f;
_sleepTime = 0.0f;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
_mass = bd.massData.mass;
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
}
_I = bd.massData.i;
if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
_invI = 1.0f / _I;
}
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
_userData = bd.userData;
_fixtureList = null;
_fixtureCount = 0;
}
// Broad-phase callback.
private void AddPair(ref FixtureProxy proxyA, ref FixtureProxy proxyB)
{
Fixture fixtureA = proxyA.Fixture;
Fixture fixtureB = proxyB.Fixture;
int indexA = proxyA.ChildIndex;
int indexB = proxyB.ChildIndex;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// Does a contact already exist?
ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.Other == bodyA)
{
Fixture fA = edge.Contact.FixtureA;
Fixture fB = edge.Contact.FixtureB;
int iA = edge.Contact.ChildIndexA;
int iB = edge.Contact.ChildIndexB;
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.Next;
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
// Contact creation may swap fixtures.
fixtureA = c.FixtureA;
fixtureB = c.FixtureB;
indexA = c.ChildIndexA;
indexB = c.ChildIndexB;
bodyA = fixtureA.Body;
bodyB = fixtureB.Body;
// Insert into the world.
c.Prev = null;
c.Next = ContactList;
if (ContactList != null)
{
ContactList.Prev = c;
}
ContactList = c;
// Connect to island graph.
// Connect to body A
c.NodeA.Contact = c;
c.NodeA.Other = bodyB;
c.NodeA.Prev = null;
c.NodeA.Next = bodyA.ContactList;
if (bodyA.ContactList != null)
{
bodyA.ContactList.Prev = c.NodeA;
}
bodyA.ContactList = c.NodeA;
// Connect to body B
c.NodeB.Contact = c;
c.NodeB.Other = bodyA;
c.NodeB.Prev = null;
c.NodeB.Next = bodyB.ContactList;
if (bodyB.ContactList != null)
{
bodyB.ContactList.Prev = c.NodeB;
}
bodyB.ContactList = c.NodeB;
++ContactCount;
}
/// <summary>
/// Destroy a fixture. This removes the fixture from the broad-phase and destroys all contacts
/// associated with this fixture. This will automatically adjust the mass of the body if the body
/// is dynamic and the fixture has positive density. All fixtures attached to a body are implicitly
/// destroyed when the body is destroyed.
/// </summary>
/// <param name="fixture">the fixture to be removed.</param>
/// <warning>This function is locked during callbacks.</warning>
public void DestroyFixture(Fixture fixture)
{
Debug.Assert(World.Locked == false);
if (World.Locked == true)
{
return;
}
Debug.Assert(fixture.Body == this);
// Remove the fixture from this body's singly linked list.
Debug.Assert(FixtureCount > 0);
Fixture node = FixtureList;
Fixture last = null; // java change
bool found = false;
while (node != null)
{
if (node == fixture)
{
node = fixture.Next;
found = true;
break;
}
last = node;
node = node.Next;
}
// You tried to remove a shape that is not attached to this body.
Debug.Assert(found);
// java change, remove it from the list
if (last == null)
{
FixtureList = fixture.Next;
}
else
{
last.Next = fixture.Next;
}
// Destroy any contacts associated with the fixture.
ContactEdge edge = ContactList;
while (edge != null)
{
Contact c = edge.Contact;
edge = edge.Next;
Fixture fixtureA = c.FixtureA;
Fixture fixtureB = c.FixtureB;
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
World.ContactManager.Destroy(c);
}
}
if ((Flags & TypeFlags.Active) == TypeFlags.Active)
{
BroadPhase broadPhase = World.ContactManager.BroadPhase;
fixture.DestroyProxies(broadPhase);
}
fixture.Destroy();
fixture.Body = null;
fixture.Next = null;
--FixtureCount;
// Reset the mass data.
ResetMassData();
}
// Broad-phase callback.
private void AddPair(ref FixtureProxy proxyA, ref FixtureProxy proxyB)
{
Fixture fixtureA = proxyA.Fixture;
Fixture fixtureB = proxyB.Fixture;
int indexA = proxyA.ChildIndex;
int indexB = proxyB.ChildIndex;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// Does a contact already exist?
ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.Other == bodyA)
{
Fixture fA = edge.Contact.FixtureA;
Fixture fB = edge.Contact.FixtureB;
int iA = edge.Contact.ChildIndexA;
int iB = edge.Contact.ChildIndexB;
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.Next;
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
//Check default filter
if (ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
{
return;
}
//FPE feature: BeforeCollision delegate
if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false)
{
return;
}
if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false)
{
return;
}
// Call the factory.
Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.FixtureA;
fixtureB = c.FixtureB;
bodyA = fixtureA.Body;
bodyB = fixtureB.Body;
// Insert into the world.
ContactList.Add(c);
#if USE_ACTIVE_CONTACT_SET
ActiveContacts.Add(c);
#endif
// Connect to island graph.
// Connect to body A
c._nodeA.Contact = c;
c._nodeA.Other = bodyB;
c._nodeA.Prev = null;
c._nodeA.Next = bodyA.ContactList;
if (bodyA.ContactList != null)
{
bodyA.ContactList.Prev = c._nodeA;
}
bodyA.ContactList = c._nodeA;
// Connect to body B
c._nodeB.Contact = c;
c._nodeB.Other = bodyA;
c._nodeB.Prev = null;
c._nodeB.Next = bodyB.ContactList;
if (bodyB.ContactList != null)
{
bodyB.ContactList.Prev = c._nodeB;
}
bodyB.ContactList = c._nodeB;
// Wake up the bodies
if (fixtureA.IsSensor == false && fixtureB.IsSensor == false)
{
bodyA.Awake = true;
bodyB.Awake = true;
}
}
/// <summary>
/// Broad-phase callback.
/// </summary>
/// <param name="proxyUserDataA"></param>
/// <param name="proxyUserDataB"></param>
public void AddPair(object proxyUserDataA, object proxyUserDataB)
{
FixtureProxy proxyA = (FixtureProxy)proxyUserDataA;
FixtureProxy proxyB = (FixtureProxy)proxyUserDataB;
Fixture fixtureA = proxyA.Fixture;
Fixture fixtureB = proxyB.Fixture;
int indexA = proxyA.ChildIndex;
int indexB = proxyB.ChildIndex;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// TODO_ERIN use a hash table to remove a potential bottleneck when both
// bodies have a lot of contacts.
// Does a contact already exist?
ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.Other == bodyA)
{
Fixture fA = edge.Contact.FixtureA;
Fixture fB = edge.Contact.FixtureB;
int iA = edge.Contact.ChildIndexA;
int iB = edge.Contact.ChildIndexB;
if (fA == fixtureA && iA == indexA && fB == fixtureB && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && iA == indexB && fB == fixtureA && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.Next;
}
// Does a joint override collision? is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
Contact c = pool.PopContact(fixtureA, indexA, fixtureB, indexB);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.FixtureA;
fixtureB = c.FixtureB;
bodyA = fixtureA.Body;
bodyB = fixtureB.Body;
// Insert into the world.
c.Prev = null;
c.Next = ContactList;
if (ContactList != null)
{
ContactList.Prev = c;
}
ContactList = c;
// Connect to island graph.
// Connect to body A
c.NodeA.Contact = c;
c.NodeA.Other = bodyB;
c.NodeA.Prev = null;
c.NodeA.Next = bodyA.ContactList;
if (bodyA.ContactList != null)
{
bodyA.ContactList.Prev = c.NodeA;
}
bodyA.ContactList = c.NodeA;
// Connect to body B
c.NodeB.Contact = c;
c.NodeB.Other = bodyA;
c.NodeB.Prev = null;
c.NodeB.Next = bodyB.ContactList;
if (bodyB.ContactList != null)
{
bodyB.ContactList.Prev = c.NodeB;
}
bodyB.ContactList = c.NodeB;
// wake up the bodies
bodyA.Awake = true;
bodyB.Awake = true;
++ContactCount;
}
public void Update(GameTime gameTime)
{
world.Step((float)gameTime.ElapsedGameTime.TotalSeconds, 10, 10);
if (world.ContactCount > 0)
{
for (ContactEdge ce = ship.GetContactList(); ce != null; ce = ce.Next)
{
bool otherHasExceeded = false;
foreach (Asteroid a in asteroids)
{
if (a.VelocityExceeded && !a.IsBodyDestroyed)
{
otherHasExceeded = true;
}
}
if (ce.Other.GetUserData() is Asteroid)
{
((Asteroid)ce.Other.GetUserData()).Destroy("Ship" + (otherHasExceeded ? "2" : ""));
}
}
for (Contact c = world.GetContactList(); c != null; c = c.GetNext())
{
Body a = c.GetFixtureA().GetBody(), b = c.GetFixtureB().GetBody();
if (a.GetUserData() is Asteroid && b.GetUserData() is Asteroid)
{
if (((Asteroid)a.GetUserData()).VelocityExceeded)
{
((Asteroid)b.GetUserData()).Destroy("Other");
}
else if (((Asteroid)b.GetUserData()).VelocityExceeded)
{
((Asteroid)a.GetUserData()).Destroy("Other");
}
}
}
}
List <Asteroid> toRemove = new List <Asteroid>();
foreach (Asteroid a in asteroids)
{
Bonus bonus = a.GetBonus();
if (bonus != Bonus.None)
{
bonuses.Add(bonus);
Score += a.Score * (int)bonus;
fuel = Math.Min(fuel + a.Score * (int)bonus * 0.0015f, MaxFuel);
}
if (a.body.Position.Y - ship.Position.Y > BackClearance)
{
a.NotVisible();
}
if (a.IsNotVisible)
{
toRemove.Add(a);
}
}
foreach (Asteroid a in toRemove)
{
asteroids.Remove(a);
}
if (fuel <= 0 && ship.GetLinearVelocity().Length() < 1f)
{
IsLevelUp = true;
}
float movement = ship.Position.Y - (cameraPosY + FrontClearance);
cameraPosY = cameraPosY + movement;
cameraPosY = MathHelper.Clamp
(cameraPosY, 0, TileHeight * Height - 600);
if (cameraPosY == 0)
{
ship.Position = new Vector2(ship.Position.X, originalShipPos.Y);
asteroidsCount += 2;
LoadAsteroidField((int)Math.Floor(asteroidsCount));
}
}
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.
if (body.JointList != null)
{
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.
if (body.FixtureList != null)
{
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);
BodyRemoved?.Invoke(body);
}
_bodyRemoveList.Clear();
}
}
private void HandleSubCollision(Contact contact, Submarine otherSub)
{
Debug.Assert(otherSub != submarine);
Vector2 normal;
FixedArray2 <Vector2> points;
contact.GetWorldManifold(out normal, out points);
if (contact.FixtureA.Body == otherSub.SubBody.Body.FarseerBody)
{
normal = -normal;
}
float thisMass = Body.Mass + submarine.DockedTo.Sum(s => s.PhysicsBody.Mass);
float otherMass = otherSub.PhysicsBody.Mass + otherSub.DockedTo.Sum(s => s.PhysicsBody.Mass);
float massRatio = otherMass / (thisMass + otherMass);
float impact = (Vector2.Dot(Velocity - otherSub.Velocity, normal) / 2.0f) * massRatio;
//apply impact to this sub (the other sub takes care of this in its own collision callback)
ApplyImpact(impact, normal, contact);
foreach (Submarine dockedSub in submarine.DockedTo)
{
dockedSub.SubBody.ApplyImpact(impact, normal, contact);
}
//find all contacts between this sub and level walls
List <Contact> levelContacts = new List <Contact>();
ContactEdge contactEdge = Body.FarseerBody.ContactList;
while (contactEdge.Next != null)
{
if (contactEdge.Contact.Enabled &&
contactEdge.Other.UserData is VoronoiCell &&
contactEdge.Contact.IsTouching)
{
levelContacts.Add(contactEdge.Contact);
}
contactEdge = contactEdge.Next;
}
if (levelContacts.Count == 0)
{
return;
}
//if this sub is in contact with the level, apply artifical impacts
//to both subs to prevent the other sub from bouncing on top of this one
//and to fake the other sub "crushing" this one against a wall
Vector2 avgContactNormal = Vector2.Zero;
foreach (Contact levelContact in levelContacts)
{
Vector2 contactNormal;
FixedArray2 <Vector2> temp;
levelContact.GetWorldManifold(out contactNormal, out temp);
//if the contact normal is pointing from the sub towards the level cell we collided with, flip the normal
VoronoiCell cell = levelContact.FixtureB.UserData is VoronoiCell ?
((VoronoiCell)levelContact.FixtureB.UserData) : ((VoronoiCell)levelContact.FixtureA.UserData);
var cellDiff = ConvertUnits.ToDisplayUnits(Body.SimPosition) - cell.Center;
if (Vector2.Dot(contactNormal, cellDiff) < 0)
{
contactNormal = -contactNormal;
}
avgContactNormal += contactNormal;
//apply impacts at the positions where this sub is touching the level
ApplyImpact((Vector2.Dot(Velocity - otherSub.Velocity, contactNormal) / 2.0f) * massRatio / levelContacts.Count, contactNormal, levelContact);
}
avgContactNormal /= levelContacts.Count;
//apply an impact to the other sub
float contactDot = Vector2.Dot(otherSub.PhysicsBody.LinearVelocity, -avgContactNormal);
if (contactDot > 0.0f)
{
otherSub.PhysicsBody.LinearVelocity -= Vector2.Normalize(otherSub.PhysicsBody.LinearVelocity) * contactDot;
impact = Vector2.Dot(otherSub.Velocity, normal);
otherSub.SubBody.ApplyImpact(impact, normal, contact);
foreach (Submarine dockedSub in otherSub.DockedTo)
{
dockedSub.SubBody.ApplyImpact(impact, normal, contact);
}
}
}
public override void earlyUpdate()
{
base.earlyUpdate();
float dd = 0;
if (neg)
{
dd = 180;
}
//Console.WriteLine(MyMath.AngleDistance(MyMath.AngleBetween(pos, obj2.pos), dd));
if (Controls.GetKey(Keys.Space) == Controls.Pressed || Controls.GetButton(Buttons.A) == Controls.Pressed)
{
ContactEdge c = body.ContactList;
if (c != null)
{
body.ApplyLinearImpulse(new Vector2(0, -150));
Console.WriteLine(c.Contact.IsTouching());
}
}
if (doc.controls.getKey(Keys.Left) == Controls.Held || Controls.GetButton(Buttons.LeftThumbstickLeft) == Controls.Held)
{
if (body.GetLinearVelocityFromLocalPoint(new Vector2(x, y)).X > -topspeed && !swinging)
{
body.ApplyLinearImpulse(new Vector2(-4.5f, 0));
}
//Console.WriteLine(body.GetLinearVelocityFromLocalPoint(new Vector2(x, y)));
neg = false;
}
if (doc.controls.getKey(Keys.Right) == Controls.Held || Controls.GetButton(Buttons.LeftThumbstickRight) == Controls.Held)
{
if (body.GetLinearVelocityFromLocalPoint(new Vector2(x, y)).X < topspeed && !swinging)
{
body.ApplyLinearImpulse(new Vector2(4.5f, 0));
}
neg = true;
}
if (Controls.GetKey(Keys.R) == Controls.Pressed || Controls.GetButton(Buttons.RightShoulder) == Controls.Pressed)
{
//whipTo(obj2);
Obj ob = null;
float dist = 700;
float ab;
float d = 0;
if (neg)
{
d = 180;
}
bool nodir = false;
float yy = y;
if (Controls.GetKey(Keys.Right) != Controls.Held && Controls.GetKey(Keys.Left) != Controls.Held)
{
nodir = true;
}
for (int i = 0; i < doc.objList.Count; i++)
{
Obj o = doc.objList[i];
ab = Math.Abs(MyMath.AngleDistance(MyMath.AngleBetween(pos, o.pos), d));
if (o.type == Obj.PointType.SwingPoint && o.body != null && MyMath.Distance(pos, o.pos) < dist && o.y < yy && (ab < 95 || nodir))
{
Console.WriteLine(ab);
dist = MyMath.Distance(pos, o.pos);
ob = o;
}
}
if (ob != null)
{
whipTo(ob);
}
}
else if (doc.controls.getKey(Keys.R) == Controls.Held)
{
//This is the Whipping which is context based.
//Search area in front of Player
//Grab nearest point and run code for it.
/*float minDist = radius;
* for (int index = 0; index < doc.objList.Count; index++)
* {
* //check distance
* if (doc.objList[index].type != PointType.Normal && MyMath.Distance(pos, doc.objList[index].pos) < minDist)
* {
* minDist = MyMath.Distance(pos, doc.objList[index].pos);
* Point = doc.objList[index];
* }
* }
* if (Point == null) return;
* if (Point.type == PointType.SwingPoint)
* {
* swing(Point);
* }
* else if (Point.type == PointType.SlingPoint)
* {
* sling(Point);
* }
* else if (Point.type == PointType.Pullblock)
* {
* pull(Point);
* }*/
}
if (Controls.GetKey(Keys.R) == Controls.Released || Controls.GetButton(Buttons.RightShoulder) == Controls.Released)
{
endWhip();
}
//Console.WriteLine(body.GetLinearVelocityFromLocalPoint(new Vector2(x, y)));
//Console.WriteLine(body.Friction);
}
