natalie/axiosengine
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

Adding initial files

Browse Source
This commit is contained in:
nathan@daedalus
2012-03-19 18:57:59 -05:00
commit 5bdc5db408
162 changed files with 43840 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

502
axios/Dynamics/Contacts/Contact.cs Normal file
View File

@@ -0,0 +1,502 @@
/*
* 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.Collision.Shapes;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Contacts
{
/// <summary>
/// A contact edge is used to connect bodies and contacts together
/// in a contact graph where each body is a node and each contact
/// is an edge. A contact edge belongs to a doubly linked list
/// maintained in each attached body. Each contact has two contact
/// nodes, one for each attached body.
/// </summary>
public sealed class ContactEdge
{
/// <summary>
/// The contact
/// </summary>
public Contact Contact;
/// <summary>
/// The next contact edge in the body's contact list
/// </summary>
public ContactEdge Next;
/// <summary>
/// Provides quick access to the other body attached.
/// </summary>
public Body Other;
/// <summary>
/// The previous contact edge in the body's contact list
/// </summary>
public ContactEdge Prev;
}
[Flags]
public enum ContactFlags
{
None = 0,
/// <summary>
/// Used when crawling contact graph when forming islands.
/// </summary>
Island = 0x0001,
/// <summary>
/// Set when the shapes are touching.
/// </summary>
Touching = 0x0002,
/// <summary>
/// This contact can be disabled (by user)
/// </summary>
Enabled = 0x0004,
/// <summary>
/// This contact needs filtering because a fixture filter was changed.
/// </summary>
Filter = 0x0008,
/// <summary>
/// This bullet contact had a TOI event
/// </summary>
BulletHit = 0x0010,
/// <summary>
/// This contact has a valid TOI i the field TOI
/// </summary>
TOI = 0x0020
}
/// <summary>
/// The class manages contact between two shapes. A contact exists for each overlapping
/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
/// that has no contact points.
/// </summary>
public class Contact
{
private static EdgeShape _edge = new EdgeShape();
private static ContactType[,] _registers = new[,]
{
{
ContactType.Circle,
ContactType.EdgeAndCircle,
ContactType.PolygonAndCircle,
ContactType.LoopAndCircle,
},
{
ContactType.EdgeAndCircle,
ContactType.NotSupported,
// 1,1 is invalid (no ContactType.Edge)
ContactType.EdgeAndPolygon,
ContactType.NotSupported,
// 1,3 is invalid (no ContactType.EdgeAndLoop)
},
{
ContactType.PolygonAndCircle,
ContactType.EdgeAndPolygon,
ContactType.Polygon,
ContactType.LoopAndPolygon,
},
{
ContactType.LoopAndCircle,
ContactType.NotSupported,
// 3,1 is invalid (no ContactType.EdgeAndLoop)
ContactType.LoopAndPolygon,
ContactType.NotSupported,
// 3,3 is invalid (no ContactType.Loop)
},
};
public Fixture FixtureA;
public Fixture FixtureB;
internal ContactFlags Flags;
public Manifold Manifold;
// Nodes for connecting bodies.
internal ContactEdge NodeA = new ContactEdge();
internal ContactEdge NodeB = new ContactEdge();
public float TOI;
internal int TOICount;
private ContactType _type;
private Contact(Fixture fA, int indexA, Fixture fB, int indexB)
{
Reset(fA, indexA, fB, indexB);
}
/// Enable/disable this contact. This can be used inside the pre-solve
/// contact listener. The contact is only disabled for the current
/// time step (or sub-step in continuous collisions).
public bool Enabled
{
set
{
if (value)
{
Flags |= ContactFlags.Enabled;
}
else
{
Flags &= ~ContactFlags.Enabled;
}
}
get { return (Flags & ContactFlags.Enabled) == ContactFlags.Enabled; }
}
/// <summary>
/// Get the child primitive index for fixture A.
/// </summary>
/// <value>The child index A.</value>
public int ChildIndexA { get; internal set; }
/// <summary>
/// Get the child primitive index for fixture B.
/// </summary>
/// <value>The child index B.</value>
public int ChildIndexB { get; internal set; }
/// <summary>
/// Get the contact manifold. Do not modify the manifold unless you understand the
/// internals of Box2D.
/// </summary>
/// <param name="manifold">The manifold.</param>
public void GetManifold(out Manifold manifold)
{
manifold = Manifold;
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out Vector2 normal, out FixedArray2<Vector2> points)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
Collision.Collision.GetWorldManifold(ref Manifold, ref bodyA.Xf, shapeA.Radius, ref bodyB.Xf, shapeB.Radius,
out normal, out points);
}
/// <summary>
/// Determines whether this contact is touching.
/// </summary>
/// <returns>
/// <c>true</c> if this instance is touching; otherwise, <c>false</c>.
/// </returns>
public bool IsTouching()
{
return (Flags & ContactFlags.Touching) == ContactFlags.Touching;
}
/// <summary>
/// Flag this contact for filtering. Filtering will occur the next time step.
/// </summary>
public void FlagForFiltering()
{
Flags |= ContactFlags.Filter;
}
private void Reset(Fixture fA, int indexA, Fixture fB, int indexB)
{
Flags = ContactFlags.Enabled;
FixtureA = fA;
FixtureB = fB;
ChildIndexA = indexA;
ChildIndexB = indexB;
Manifold.PointCount = 0;
NodeA.Contact = null;
NodeA.Prev = null;
NodeA.Next = null;
NodeA.Other = null;
NodeB.Contact = null;
NodeB.Prev = null;
NodeB.Next = null;
NodeB.Other = null;
TOICount = 0;
}
/// <summary>
/// Update the contact manifold and touching status.
/// Note: do not assume the fixture AABBs are overlapping or are valid.
/// </summary>
/// <param name="contactManager">The contact manager.</param>
internal void Update(ContactManager contactManager)
{
Manifold oldManifold = Manifold;
// Re-enable this contact.
Flags |= ContactFlags.Enabled;
bool touching;
bool wasTouching = (Flags & ContactFlags.Touching) == ContactFlags.Touching;
bool sensor = FixtureA.IsSensor || FixtureB.IsSensor;
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
// Is this contact a sensor?
if (sensor)
{
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
touching = AABB.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA.Xf, ref bodyB.Xf);
// Sensors don't generate manifolds.
Manifold.PointCount = 0;
}
else
{
Evaluate(ref Manifold, ref bodyA.Xf, ref bodyB.Xf);
touching = Manifold.PointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < Manifold.PointCount; ++i)
{
ManifoldPoint mp2 = Manifold.Points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.Id;
bool found = false;
for (int j = 0; j < oldManifold.PointCount; ++j)
{
ManifoldPoint mp1 = oldManifold.Points[j];
if (mp1.Id.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
found = true;
break;
}
}
if (found == false)
{
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
}
Manifold.Points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.Awake = true;
bodyB.Awake = true;
}
}
if (touching)
{
Flags |= ContactFlags.Touching;
}
else
{
Flags &= ~ContactFlags.Touching;
}
if (wasTouching == false && touching)
{
//Report the collision to both participants:
if (FixtureA.OnCollision != null)
Enabled = FixtureA.OnCollision(FixtureA, FixtureB, this);
//Reverse the order of the reported fixtures. The first fixture is always the one that the
//user subscribed to.
if (FixtureB.OnCollision != null)
Enabled = FixtureB.OnCollision(FixtureB, FixtureA, this);
//BeginContact can also return false and disable the contact
if (contactManager.BeginContact != null)
Enabled = contactManager.BeginContact(this);
//if the user disabled the contact (needed to exclude it in TOI solver), we also need to mark
//it as not touching.
if (Enabled == false)
Flags &= ~ContactFlags.Touching;
}
if (wasTouching && touching == false)
{
//Report the separation to both participants:
if (FixtureA != null && FixtureA.OnSeparation != null)
FixtureA.OnSeparation(FixtureA, FixtureB);
//Reverse the order of the reported fixtures. The first fixture is always the one that the
//user subscribed to.
if (FixtureB != null && FixtureB.OnSeparation != null)
FixtureB.OnSeparation(FixtureB, FixtureA);
if (contactManager.EndContact != null)
contactManager.EndContact(this);
}
if (sensor)
return;
if (contactManager.PreSolve != null)
contactManager.PreSolve(this, ref oldManifold);
}
/// <summary>
/// Evaluate this contact with your own manifold and transforms.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="transformA">The first transform.</param>
/// <param name="transformB">The second transform.</param>
private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB)
{
switch (_type)
{
case ContactType.Polygon:
Collision.Collision.CollidePolygons(ref manifold,
(PolygonShape)FixtureA.Shape, ref transformA,
(PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.PolygonAndCircle:
Collision.Collision.CollidePolygonAndCircle(ref manifold,
(PolygonShape)FixtureA.Shape, ref transformA,
(CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.EdgeAndCircle:
Collision.Collision.CollideEdgeAndCircle(ref manifold,
(EdgeShape)FixtureA.Shape, ref transformA,
(CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.EdgeAndPolygon:
Collision.Collision.CollideEdgeAndPolygon(ref manifold,
(EdgeShape)FixtureA.Shape, ref transformA,
(PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.LoopAndCircle:
LoopShape loop = (LoopShape)FixtureA.Shape;
loop.GetChildEdge(ref _edge, ChildIndexA);
Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA,
(CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.LoopAndPolygon:
LoopShape loop2 = (LoopShape)FixtureA.Shape;
loop2.GetChildEdge(ref _edge, ChildIndexA);
Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA,
(PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.Circle:
Collision.Collision.CollideCircles(ref manifold,
(CircleShape)FixtureA.Shape, ref transformA,
(CircleShape)FixtureB.Shape, ref transformB);
break;
}
}
internal static Contact Create(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
{
ShapeType type1 = fixtureA.ShapeType;
ShapeType type2 = fixtureB.ShapeType;
Debug.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount);
Debug.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount);
Contact c;
Queue<Contact> pool = fixtureA.Body.World.ContactPool;
if (pool.Count > 0)
{
c = pool.Dequeue();
if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon))
&&
!(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
{
c.Reset(fixtureA, indexA, fixtureB, indexB);
}
else
{
c.Reset(fixtureB, indexB, fixtureA, indexA);
}
}
else
{
// Edge+Polygon is non-symetrical due to the way Erin handles collision type registration.
if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon))
&&
!(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
{
c = new Contact(fixtureA, indexA, fixtureB, indexB);
}
else
{
c = new Contact(fixtureB, indexB, fixtureA, indexA);
}
}
c._type = _registers[(int)type1, (int)type2];
return c;
}
internal void Destroy()
{
FixtureA.Body.World.ContactPool.Enqueue(this);
Reset(null, 0, null, 0);
}
#region Nested type: ContactType
private enum ContactType
{
NotSupported,
Polygon,
PolygonAndCircle,
Circle,
EdgeAndPolygon,
EdgeAndCircle,
LoopAndPolygon,
LoopAndCircle,
}
#endregion
}
}

794
axios/Dynamics/Contacts/ContactSolver.cs Normal file
View File

@@ -0,0 +1,794 @@
/*
* 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.Diagnostics;
using FarseerPhysics.Collision;
using FarseerPhysics.Collision.Shapes;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Contacts
{
public sealed class ContactConstraintPoint
{
public Vector2 LocalPoint;
public float NormalImpulse;
public float NormalMass;
public float TangentImpulse;
public float TangentMass;
public float VelocityBias;
public Vector2 rA;
public Vector2 rB;
}
public sealed class ContactConstraint
{
public Body BodyA;
public Body BodyB;
public float Friction;
public Mat22 K;
public Vector2 LocalNormal;
public Vector2 LocalPoint;
public Manifold Manifold;
public Vector2 Normal;
public Mat22 NormalMass;
public int PointCount;
public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxPolygonVertices];
public float RadiusA;
public float RadiusB;
public float Restitution;
public ManifoldType Type;
public ContactConstraint()
{
for (int i = 0; i < Settings.MaxManifoldPoints; i++)
{
Points[i] = new ContactConstraintPoint();
}
}
}
public class ContactSolver
{
public ContactConstraint[] Constraints;
private int _constraintCount; // collection can be bigger.
private Contact[] _contacts;
public void Reset(Contact[] contacts, int contactCount, float impulseRatio, bool warmstarting)
{
_contacts = contacts;
_constraintCount = contactCount;
// grow the array
if (Constraints == null || Constraints.Length < _constraintCount)
{
Constraints = new ContactConstraint[_constraintCount * 2];
for (int i = 0; i < Constraints.Length; i++)
{
Constraints[i] = new ContactConstraint();
}
}
// Initialize position independent portions of the constraints.
for (int i = 0; i < _constraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
Debug.Assert(manifold.PointCount > 0);
ContactConstraint cc = Constraints[i];
cc.Friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction);
cc.Restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution);
cc.BodyA = bodyA;
cc.BodyB = bodyB;
cc.Manifold = manifold;
cc.Normal = Vector2.Zero;
cc.PointCount = manifold.PointCount;
cc.LocalNormal = manifold.LocalNormal;
cc.LocalPoint = manifold.LocalPoint;
cc.RadiusA = radiusA;
cc.RadiusB = radiusB;
cc.Type = manifold.Type;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = cc.Points[j];
if (warmstarting)
{
ccp.NormalImpulse = impulseRatio * cp.NormalImpulse;
ccp.TangentImpulse = impulseRatio * cp.TangentImpulse;
}
else
{
ccp.NormalImpulse = 0.0f;
ccp.TangentImpulse = 0.0f;
}
ccp.LocalPoint = cp.LocalPoint;
ccp.rA = Vector2.Zero;
ccp.rB = Vector2.Zero;
ccp.NormalMass = 0.0f;
ccp.TangentMass = 0.0f;
ccp.VelocityBias = 0.0f;
}
cc.K.SetZero();
cc.NormalMass.SetZero();
}
}
public void InitializeVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint cc = Constraints[i];
float radiusA = cc.RadiusA;
float radiusB = cc.RadiusB;
Body bodyA = cc.BodyA;
Body bodyB = cc.BodyB;
Manifold manifold = cc.Manifold;
Vector2 vA = bodyA.LinearVelocity;
Vector2 vB = bodyB.LinearVelocity;
float wA = bodyA.AngularVelocity;
float wB = bodyB.AngularVelocity;
Debug.Assert(manifold.PointCount > 0);
FixedArray2<Vector2> points;
Collision.Collision.GetWorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf, radiusB,
out cc.Normal, out points);
Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X);
for (int j = 0; j < cc.PointCount; ++j)
{
ContactConstraintPoint ccp = cc.Points[j];
ccp.rA = points[j] - bodyA.Sweep.C;
ccp.rB = points[j] - bodyB.Sweep.C;
float rnA = ccp.rA.X * cc.Normal.Y - ccp.rA.Y * cc.Normal.X;
float rnB = ccp.rB.X * cc.Normal.Y - ccp.rB.Y * cc.Normal.X;
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB;
Debug.Assert(kNormal > Settings.Epsilon);
ccp.NormalMass = 1.0f / kNormal;
float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X;
float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X;
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB;
Debug.Assert(kTangent > Settings.Epsilon);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = cc.Normal.X * (vB.X + -wB * ccp.rB.Y - vA.X - -wA * ccp.rA.Y) +
cc.Normal.Y * (vB.Y + wB * ccp.rB.X - vA.Y - wA * ccp.rA.X);
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = cc.Points[0];
ContactConstraintPoint ccp2 = cc.Points[1];
float invMassA = bodyA.InvMass;
float invIA = bodyA.InvI;
float invMassB = bodyB.InvMass;
float invIB = bodyB.InvI;
float rn1A = ccp1.rA.X * cc.Normal.Y - ccp1.rA.Y * cc.Normal.X;
float rn1B = ccp1.rB.X * cc.Normal.Y - ccp1.rB.Y * cc.Normal.X;
float rn2A = ccp2.rA.X * cc.Normal.Y - ccp2.rA.Y * cc.Normal.X;
float rn2B = ccp2.rB.X * cc.Normal.Y - ccp2.rB.Y * cc.Normal.X;
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.Col1.X = k11;
cc.K.Col1.Y = k12;
cc.K.Col2.X = k12;
cc.K.Col2.Y = k22;
float a = cc.K.Col1.X, b = cc.K.Col2.X, c = cc.K.Col1.Y, d = cc.K.Col2.Y;
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
cc.NormalMass.Col1.X = det * d;
cc.NormalMass.Col1.Y = -det * c;
cc.NormalMass.Col2.X = -det * b;
cc.NormalMass.Col2.Y = det * a;
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
}
public void WarmStart()
{
// Warm start.
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
float tangentx = c.Normal.Y;
float tangenty = -c.Normal.X;
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
float px = ccp.NormalImpulse * c.Normal.X + ccp.TangentImpulse * tangentx;
float py = ccp.NormalImpulse * c.Normal.Y + ccp.TangentImpulse * tangenty;
c.BodyA.AngularVelocityInternal -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px);
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py;
c.BodyB.AngularVelocityInternal += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px);
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py;
}
}
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
float wA = c.BodyA.AngularVelocityInternal;
float wB = c.BodyB.AngularVelocityInternal;
float tangentx = c.Normal.Y;
float tangenty = -c.Normal.X;
float friction = c.Friction;
Debug.Assert(c.PointCount == 1 || c.PointCount == 2);
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
float lambda = ccp.TangentMass *
-((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) -
c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * tangentx +
(c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) -
c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * tangenty);
// MathUtils.Clamp the accumulated force
float maxFriction = friction * ccp.NormalImpulse;
float newImpulse = Math.Max(-maxFriction, Math.Min(ccp.TangentImpulse + lambda, maxFriction));
lambda = newImpulse - ccp.TangentImpulse;
// Apply contact impulse
float px = lambda * tangentx;
float py = lambda * tangenty;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py;
wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px);
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py;
wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px);
ccp.TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
// Compute normal impulse
float lambda = -ccp.NormalMass *
((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) -
c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * c.Normal.X +
(c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) -
c.BodyA.LinearVelocityInternal.Y -
(wA * ccp.rA.X)) * c.Normal.Y - ccp.VelocityBias);
// Clamp the accumulated impulse
float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
float px = lambda * c.Normal.X;
float py = lambda * c.Normal.Y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py;
wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px);
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py;
wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px);
ccp.NormalImpulse = newImpulse;
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
ContactConstraintPoint cp1 = c.Points[0];
ContactConstraintPoint cp2 = c.Points[1];
float ax = cp1.NormalImpulse;
float ay = cp2.NormalImpulse;
Debug.Assert(ax >= 0.0f && ay >= 0.0f);
// Relative velocity at contact
// Compute normal velocity
float vn1 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp1.rB.Y) - c.BodyA.LinearVelocityInternal.X -
(-wA * cp1.rA.Y)) * c.Normal.X +
(c.BodyB.LinearVelocityInternal.Y + (wB * cp1.rB.X) - c.BodyA.LinearVelocityInternal.Y -
(wA * cp1.rA.X)) * c.Normal.Y;
float vn2 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp2.rB.Y) - c.BodyA.LinearVelocityInternal.X -
(-wA * cp2.rA.Y)) * c.Normal.X +
(c.BodyB.LinearVelocityInternal.Y + (wB * cp2.rB.X) - c.BodyA.LinearVelocityInternal.Y -
(wA * cp2.rA.X)) * c.Normal.Y;
float bx = vn1 - cp1.VelocityBias - (c.K.Col1.X * ax + c.K.Col2.X * ay);
float by = vn2 - cp2.VelocityBias - (c.K.Col1.Y * ax + c.K.Col2.Y * ay);
float xx = -(c.NormalMass.Col1.X * bx + c.NormalMass.Col2.X * by);
float xy = -(c.NormalMass.Col1.Y * bx + c.NormalMass.Col2.Y * by);
while (true)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
if (xx >= 0.0f && xy >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.X;
float p1y = dx * c.Normal.Y;
float p2x = dy * c.Normal.X;
float p2y = dy * c.Normal.Y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
float k_errorTol = 1e-3f;
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
vn2 = Vector2.Dot(dv2, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + b2'
//
xx = -cp1.NormalMass * bx;
xy = 0.0f;
vn1 = 0.0f;
vn2 = c.K.Col1.Y * xx + by;
if (xx >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.X;
float p1y = dx * c.Normal.Y;
float p2x = dy * c.Normal.X;
float p2y = dy * c.Normal.Y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + b2'
//
xx = 0.0f;
xy = -cp2.NormalMass * by;
vn1 = c.K.Col2.X * xy + bx;
vn2 = 0.0f;
if (xy >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.X;
float p1y = dx * c.Normal.Y;
float p2x = dy * c.Normal.X;
float p2y = dy * c.Normal.Y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
// Postconditions
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn2 = Vector2.Dot(dv2, normal);
Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
xx = 0.0f;
xy = 0.0f;
vn1 = bx;
vn2 = by;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.X;
float p1y = dx * c.Normal.Y;
float p2x = dy * c.Normal.X;
float p2y = dy * c.Normal.Y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
c.BodyA.AngularVelocityInternal = wA;
c.BodyB.AngularVelocityInternal = wB;
}
}
public void StoreImpulses()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Manifold m = c.Manifold;
for (int j = 0; j < c.PointCount; ++j)
{
ManifoldPoint pj = m.Points[j];
ContactConstraintPoint cp = c.Points[j];
pj.NormalImpulse = cp.NormalImpulse;
pj.TangentImpulse = cp.TangentImpulse;
m.Points[j] = pj;
}
c.Manifold = m;
_contacts[i].Manifold = m;
}
}
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA.Mass * bodyA.InvMass;
float invIA = bodyA.Mass * bodyA.InvI;
float invMassB = bodyB.Mass * bodyB.InvMass;
float invIB = bodyB.Mass * bodyB.InvI;
// Solve normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
Vector2 normal;
Vector2 point;
float separation;
Solve(c, j, out normal, out point, out separation);
float rax = point.X - bodyA.Sweep.C.X;
float ray = point.Y - bodyA.Sweep.C.Y;
float rbx = point.X - bodyB.Sweep.C.X;
float rby = point.Y - bodyB.Sweep.C.Y;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Math.Max(-Settings.MaxLinearCorrection,
Math.Min(baumgarte * (separation + Settings.LinearSlop), 0.0f));
// Compute the effective mass.
float rnA = rax * normal.Y - ray * normal.X;
float rnB = rbx * normal.Y - rby * normal.X;
float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
float px = impulse * normal.X;
float py = impulse * normal.Y;
bodyA.Sweep.C.X -= invMassA * px;
bodyA.Sweep.C.Y -= invMassA * py;
bodyA.Sweep.A -= invIA * (rax * py - ray * px);
bodyB.Sweep.C.X += invMassB * px;
bodyB.Sweep.C.Y += invMassB * py;
bodyB.Sweep.A += invIB * (rbx * py - rby * px);
bodyA.SynchronizeTransform();
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't
// push the separation above -Settings.b2_linearSlop.
return minSeparation >= -1.5f * Settings.LinearSlop;
}
private static void Solve(ContactConstraint cc, int index, out Vector2 normal, out Vector2 point,
out float separation)
{
Debug.Assert(cc.PointCount > 0);
normal = Vector2.Zero;
switch (cc.Type)
{
case ManifoldType.Circles:
{
Vector2 pointA = cc.BodyA.GetWorldPoint(ref cc.LocalPoint);
Vector2 pointB = cc.BodyB.GetWorldPoint(ref cc.Points[0].LocalPoint);
float a = (pointA.X - pointB.X) * (pointA.X - pointB.X) +
(pointA.Y - pointB.Y) * (pointA.Y - pointB.Y);
if (a > Settings.Epsilon * Settings.Epsilon)
{
Vector2 normalTmp = pointB - pointA;
float factor = 1f / (float)Math.Sqrt(normalTmp.X * normalTmp.X + normalTmp.Y * normalTmp.Y);
normal.X = normalTmp.X * factor;
normal.Y = normalTmp.Y * factor;
}
else
{
normal.X = 1;
normal.Y = 0;
}
point = 0.5f * (pointA + pointB);
separation = (pointB.X - pointA.X) * normal.X + (pointB.Y - pointA.Y) * normal.Y - cc.RadiusA -
cc.RadiusB;
}
break;
case ManifoldType.FaceA:
{
normal = cc.BodyA.GetWorldVector(ref cc.LocalNormal);
Vector2 planePoint = cc.BodyA.GetWorldPoint(ref cc.LocalPoint);
Vector2 clipPoint = cc.BodyB.GetWorldPoint(ref cc.Points[index].LocalPoint);
separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y -
cc.RadiusA - cc.RadiusB;
point = clipPoint;
}
break;
case ManifoldType.FaceB:
{
normal = cc.BodyB.GetWorldVector(ref cc.LocalNormal);
Vector2 planePoint = cc.BodyB.GetWorldPoint(ref cc.LocalPoint);
Vector2 clipPoint = cc.BodyA.GetWorldPoint(ref cc.Points[index].LocalPoint);
separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y -
cc.RadiusA - cc.RadiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
default:
point = Vector2.Zero;
separation = 0.0f;
break;
}
}
}
}