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

1388
axios/Dynamics/Body.cs Normal file
View File

File diff suppressed because it is too large Load Diff

142
axios/Dynamics/BreakableBody.cs Normal file
View File

@@ -0,0 +1,142 @@
using System;
using System.Collections.Generic;
using FarseerPhysics.Collision.Shapes;
using FarseerPhysics.Common;
using FarseerPhysics.Dynamics.Contacts;
using FarseerPhysics.Factories;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics
{
/// <summary>
/// A type of body that supports multiple fixtures that can break apart.
/// </summary>
public class BreakableBody
{
public bool Broken;
public Body MainBody;
public List<Fixture> Parts = new List<Fixture>(8);
/// <summary>
/// The force needed to break the body apart.
/// Default: 500
/// </summary>
public float Strength = 500.0f;
private float[] _angularVelocitiesCache = new float[8];
private bool _break;
private Vector2[] _velocitiesCache = new Vector2[8];
private World _world;
public BreakableBody(IEnumerable<Vertices> vertices, World world, float density)
: this(vertices, world, density, null)
{
}
public BreakableBody()
{
}
public BreakableBody(IEnumerable<Vertices> vertices, World world, float density, object userData)
{
_world = world;
_world.ContactManager.PostSolve += PostSolve;
MainBody = new Body(_world);
MainBody.BodyType = BodyType.Dynamic;
foreach (Vertices part in vertices)
{
PolygonShape polygonShape = new PolygonShape(part, density);
Fixture fixture = MainBody.CreateFixture(polygonShape, userData);
Parts.Add(fixture);
}
}
private void PostSolve(Contact contact, ContactConstraint impulse)
{
if (!Broken)
{
if (Parts.Contains(contact.FixtureA) || Parts.Contains(contact.FixtureB))
{
float maxImpulse = 0.0f;
int count = contact.Manifold.PointCount;
for (int i = 0; i < count; ++i)
{
maxImpulse = Math.Max(maxImpulse, impulse.Points[i].NormalImpulse);
}
if (maxImpulse > Strength)
{
// Flag the body for breaking.
_break = true;
}
}
}
}
public void Update()
{
if (_break)
{
Decompose();
Broken = true;
_break = false;
}
// Cache velocities to improve movement on breakage.
if (Broken == false)
{
//Enlarge the cache if needed
if (Parts.Count > _angularVelocitiesCache.Length)
{
_velocitiesCache = new Vector2[Parts.Count];
_angularVelocitiesCache = new float[Parts.Count];
}
//Cache the linear and angular velocities.
for (int i = 0; i < Parts.Count; i++)
{
_velocitiesCache[i] = Parts[i].Body.LinearVelocity;
_angularVelocitiesCache[i] = Parts[i].Body.AngularVelocity;
}
}
}
private void Decompose()
{
//Unsubsribe from the PostSolve delegate
_world.ContactManager.PostSolve -= PostSolve;
for (int i = 0; i < Parts.Count; i++)
{
Fixture fixture = Parts[i];
Shape shape = fixture.Shape.Clone();
object userdata = fixture.UserData;
MainBody.DestroyFixture(fixture);
Body body = BodyFactory.CreateBody(_world);
body.BodyType = BodyType.Dynamic;
body.Position = MainBody.Position;
body.Rotation = MainBody.Rotation;
body.UserData = MainBody.UserData;
body.CreateFixture(shape, userdata);
body.AngularVelocity = _angularVelocitiesCache[i];
body.LinearVelocity = _velocitiesCache[i];
}
_world.RemoveBody(MainBody);
_world.RemoveBreakableBody(this);
}
public void Break()
{
_break = true;
}
}
}

340
axios/Dynamics/ContactManager.cs Normal file
View File

@@ -0,0 +1,340 @@
/*
* 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.Collections.Generic;
using FarseerPhysics.Collision;
using FarseerPhysics.Dynamics.Contacts;
namespace FarseerPhysics.Dynamics
{
public class ContactManager
{
/// <summary>
/// Fires when a contact is created
/// </summary>
public BeginContactDelegate BeginContact;
public IBroadPhase BroadPhase;
/// <summary>
/// The filter used by the contact manager.
/// </summary>
public CollisionFilterDelegate ContactFilter;
public List<Contact> ContactList = new List<Contact>(128);
/// <summary>
/// Fires when a contact is deleted
/// </summary>
public EndContactDelegate EndContact;
/// <summary>
/// Fires when the broadphase detects that two Fixtures are close to each other.
/// </summary>
public BroadphaseDelegate OnBroadphaseCollision;
/// <summary>
/// Fires after the solver has run
/// </summary>
public PostSolveDelegate PostSolve;
/// <summary>
/// Fires before the solver runs
/// </summary>
public PreSolveDelegate PreSolve;
internal ContactManager(IBroadPhase broadPhase)
{
BroadPhase = broadPhase;
OnBroadphaseCollision = AddPair;
}
// 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;
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);
// Contact creation may swap fixtures.
fixtureA = c.FixtureA;
fixtureB = c.FixtureB;
bodyA = fixtureA.Body;
bodyB = fixtureB.Body;
// Insert into the world.
ContactList.Add(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;
}
internal void FindNewContacts()
{
BroadPhase.UpdatePairs(OnBroadphaseCollision);
}
internal void Destroy(Contact contact)
{
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
if (EndContact != null && contact.IsTouching())
{
EndContact(contact);
}
// Remove from the world.
ContactList.Remove(contact);
// Remove from body 1
if (contact.NodeA.Prev != null)
{
contact.NodeA.Prev.Next = contact.NodeA.Next;
}
if (contact.NodeA.Next != null)
{
contact.NodeA.Next.Prev = contact.NodeA.Prev;
}
if (contact.NodeA == bodyA.ContactList)
{
bodyA.ContactList = contact.NodeA.Next;
}
// Remove from body 2
if (contact.NodeB.Prev != null)
{
contact.NodeB.Prev.Next = contact.NodeB.Next;
}
if (contact.NodeB.Next != null)
{
contact.NodeB.Next.Prev = contact.NodeB.Prev;
}
if (contact.NodeB == bodyB.ContactList)
{
bodyB.ContactList = contact.NodeB.Next;
}
contact.Destroy();
}
internal void Collide()
{
// Update awake contacts.
for (int i = 0; i < ContactList.Count; i++)
{
Contact c = ContactList[i];
Fixture fixtureA = c.FixtureA;
Fixture fixtureB = c.FixtureB;
int indexA = c.ChildIndexA;
int indexB = c.ChildIndexB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
if (bodyA.Awake == false && bodyB.Awake == false)
{
continue;
}
// Is this contact flagged for filtering?
if ((c.Flags & ContactFlags.Filter) == ContactFlags.Filter)
{
// Should these bodies collide?
if (bodyB.ShouldCollide(bodyA) == false)
{
Contact cNuke = c;
Destroy(cNuke);
continue;
}
// Check default filtering
if (ShouldCollide(fixtureA, fixtureB) == false)
{
Contact cNuke = c;
Destroy(cNuke);
continue;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
{
Contact cNuke = c;
Destroy(cNuke);
continue;
}
// Clear the filtering flag.
c.Flags &= ~ContactFlags.Filter;
}
int proxyIdA = fixtureA.Proxies[indexA].ProxyId;
int proxyIdB = fixtureB.Proxies[indexB].ProxyId;
bool overlap = BroadPhase.TestOverlap(proxyIdA, proxyIdB);
// Here we destroy contacts that cease to overlap in the broad-phase.
if (overlap == false)
{
Contact cNuke = c;
Destroy(cNuke);
continue;
}
// The contact persists.
c.Update(this);
}
}
private static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
{
if (Settings.UseFPECollisionCategories)
{
if ((fixtureA.CollisionGroup == fixtureB.CollisionGroup) &&
fixtureA.CollisionGroup != 0 && fixtureB.CollisionGroup != 0)
return false;
if (((fixtureA.CollisionCategories & fixtureB.CollidesWith) ==
Category.None) &
((fixtureB.CollisionCategories & fixtureA.CollidesWith) ==
Category.None))
return false;
if (fixtureA.IsFixtureIgnored(fixtureB) ||
fixtureB.IsFixtureIgnored(fixtureA))
return false;
return true;
}
if (fixtureA.CollisionGroup == fixtureB.CollisionGroup &&
fixtureA.CollisionGroup != 0)
{
return fixtureA.CollisionGroup > 0;
}
bool collide = (fixtureA.CollidesWith & fixtureB.CollisionCategories) != 0 &&
(fixtureA.CollisionCategories & fixtureB.CollidesWith) != 0;
if (collide)
{
if (fixtureA.IsFixtureIgnored(fixtureB) ||
fixtureB.IsFixtureIgnored(fixtureA))
{
return false;
}
}
return collide;
}
}
}

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;
}
}
}
}

611
axios/Dynamics/Fixture.cs Normal file
View File

@@ -0,0 +1,611 @@
/*
* 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 FarseerPhysics.Dynamics.Contacts;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics
{
[Flags]
public enum Category
{
None = 0,
All = int.MaxValue,
Cat1 = 1,
Cat2 = 2,
Cat3 = 4,
Cat4 = 8,
Cat5 = 16,
Cat6 = 32,
Cat7 = 64,
Cat8 = 128,
Cat9 = 256,
Cat10 = 512,
Cat11 = 1024,
Cat12 = 2048,
Cat13 = 4096,
Cat14 = 8192,
Cat15 = 16384,
Cat16 = 32768,
Cat17 = 65536,
Cat18 = 131072,
Cat19 = 262144,
Cat20 = 524288,
Cat21 = 1048576,
Cat22 = 2097152,
Cat23 = 4194304,
Cat24 = 8388608,
Cat25 = 16777216,
Cat26 = 33554432,
Cat27 = 67108864,
Cat28 = 134217728,
Cat29 = 268435456,
Cat30 = 536870912,
Cat31 = 1073741824
}
/// <summary>
/// This proxy is used internally to connect fixtures to the broad-phase.
/// </summary>
public struct FixtureProxy
{
public AABB AABB;
public int ChildIndex;
public Fixture Fixture;
public int ProxyId;
}
/// <summary>
/// A fixture is used to attach a Shape to a body for collision detection. A fixture
/// inherits its transform from its parent. Fixtures hold additional non-geometric data
/// such as friction, collision filters, etc.
/// Fixtures are created via Body.CreateFixture.
/// Warning: You cannot reuse fixtures.
/// </summary>
public class Fixture : IDisposable
{
private static int _fixtureIdCounter;
/// <summary>
/// Fires after two shapes has collided and are solved. This gives you a chance to get the impact force.
/// </summary>
public AfterCollisionEventHandler AfterCollision;
/// <summary>
/// Fires when two fixtures are close to each other.
/// Due to how the broadphase works, this can be quite inaccurate as shapes are approximated using AABBs.
/// </summary>
public BeforeCollisionEventHandler BeforeCollision;
/// <summary>
/// Fires when two shapes collide and a contact is created between them.
/// Note that the first fixture argument is always the fixture that the delegate is subscribed to.
/// </summary>
public OnCollisionEventHandler OnCollision;
/// <summary>
/// Fires when two shapes separate and a contact is removed between them.
/// Note that the first fixture argument is always the fixture that the delegate is subscribed to.
/// </summary>
public OnSeparationEventHandler OnSeparation;
public FixtureProxy[] Proxies;
public int ProxyCount;
internal Category _collidesWith;
internal Category _collisionCategories;
internal short _collisionGroup;
internal Dictionary<int, bool> _collisionIgnores;
private float _friction;
private float _restitution;
internal Fixture()
{
}
public Fixture(Body body, Shape shape)
: this(body, shape, null)
{
}
public Fixture(Body body, Shape shape, object userData)
{
if (Settings.UseFPECollisionCategories)
_collisionCategories = Category.All;
else
_collisionCategories = Category.Cat1;
_collidesWith = Category.All;
_collisionGroup = 0;
//Fixture defaults
Friction = 0.2f;
Restitution = 0;
IsSensor = false;
Body = body;
UserData = userData;
#pragma warning disable 162
if (Settings.ConserveMemory)
Shape = shape;
else
Shape = shape.Clone();
#pragma warning restore 162
RegisterFixture();
}
/// <summary>
/// Defaults to 0
///
/// If Settings.UseFPECollisionCategories is set to false:
/// Collision groups allow a certain group of objects to never collide (negative)
/// or always collide (positive). Zero means no collision group. Non-zero group
/// filtering always wins against the mask bits.
///
/// If Settings.UseFPECollisionCategories is set to true:
/// If 2 fixtures are in the same collision group, they will not collide.
/// </summary>
public short CollisionGroup
{
set
{
if (_collisionGroup == value)
return;
_collisionGroup = value;
Refilter();
}
get { return _collisionGroup; }
}
/// <summary>
/// Defaults to Category.All
///
/// The collision mask bits. This states the categories that this
/// fixture would accept for collision.
/// Use Settings.UseFPECollisionCategories to change the behavior.
/// </summary>
public Category CollidesWith
{
get { return _collidesWith; }
set
{
if (_collidesWith == value)
return;
_collidesWith = value;
Refilter();
}
}
/// <summary>
/// The collision categories this fixture is a part of.
///
/// If Settings.UseFPECollisionCategories is set to false:
/// Defaults to Category.Cat1
///
/// If Settings.UseFPECollisionCategories is set to true:
/// Defaults to Category.All
/// </summary>
public Category CollisionCategories
{
get { return _collisionCategories; }
set
{
if (_collisionCategories == value)
return;
_collisionCategories = value;
Refilter();
}
}
/// <summary>
/// Get the type of the child Shape. You can use this to down cast to the concrete Shape.
/// </summary>
/// <value>The type of the shape.</value>
public ShapeType ShapeType
{
get { return Shape.ShapeType; }
}
/// <summary>
/// Get the child Shape. You can modify the child Shape, however you should not change the
/// number of vertices because this will crash some collision caching mechanisms.
/// </summary>
/// <value>The shape.</value>
public Shape Shape { get; internal set; }
/// <summary>
/// Gets or sets a value indicating whether this fixture is a sensor.
/// </summary>
/// <value><c>true</c> if this instance is a sensor; otherwise, <c>false</c>.</value>
public bool IsSensor { get; set; }
/// <summary>
/// Get the parent body of this fixture. This is null if the fixture is not attached.
/// </summary>
/// <value>The body.</value>
public Body Body { get; internal set; }
/// <summary>
/// Set the user data. Use this to store your application specific data.
/// </summary>
/// <value>The user data.</value>
public object UserData { get; set; }
/// <summary>
/// Get or set the coefficient of friction.
/// </summary>
/// <value>The friction.</value>
public float Friction
{
get { return _friction; }
set
{
Debug.Assert(!float.IsNaN(value));
_friction = value;
}
}
/// <summary>
/// Get or set the coefficient of restitution.
/// </summary>
/// <value>The restitution.</value>
public float Restitution
{
get { return _restitution; }
set
{
Debug.Assert(!float.IsNaN(value));
_restitution = value;
}
}
/// <summary>
/// Gets a unique ID for this fixture.
/// </summary>
/// <value>The fixture id.</value>
public int FixtureId { get; private set; }
#region IDisposable Members
public bool IsDisposed { get; set; }
public void Dispose()
{
if (!IsDisposed)
{
Body.DestroyFixture(this);
IsDisposed = true;
GC.SuppressFinalize(this);
}
}
#endregion
/// <summary>
/// Restores collisions between this fixture and the provided fixture.
/// </summary>
/// <param name="fixture">The fixture.</param>
public void RestoreCollisionWith(Fixture fixture)
{
if (_collisionIgnores == null)
return;
if (_collisionIgnores.ContainsKey(fixture.FixtureId))
{
_collisionIgnores[fixture.FixtureId] = false;
Refilter();
}
}
/// <summary>
/// Ignores collisions between this fixture and the provided fixture.
/// </summary>
/// <param name="fixture">The fixture.</param>
public void IgnoreCollisionWith(Fixture fixture)
{
if (_collisionIgnores == null)
_collisionIgnores = new Dictionary<int, bool>();
if (_collisionIgnores.ContainsKey(fixture.FixtureId))
_collisionIgnores[fixture.FixtureId] = true;
else
_collisionIgnores.Add(fixture.FixtureId, true);
Refilter();
}
/// <summary>
/// Determines whether collisions are ignored between this fixture and the provided fixture.
/// </summary>
/// <param name="fixture">The fixture.</param>
/// <returns>
/// <c>true</c> if the fixture is ignored; otherwise, <c>false</c>.
/// </returns>
public bool IsFixtureIgnored(Fixture fixture)
{
if (_collisionIgnores == null)
return false;
if (_collisionIgnores.ContainsKey(fixture.FixtureId))
return _collisionIgnores[fixture.FixtureId];
return false;
}
/// <summary>
/// Contacts are persistant and will keep being persistant unless they are
/// flagged for filtering.
/// This methods flags all contacts associated with the body for filtering.
/// </summary>
internal void Refilter()
{
// 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.FlagForFiltering();
}
edge = edge.Next;
}
World world = Body.World;
if (world == null)
{
return;
}
// Touch each proxy so that new pairs may be created
IBroadPhase broadPhase = world.ContactManager.BroadPhase;
for (int i = 0; i < ProxyCount; ++i)
{
broadPhase.TouchProxy(Proxies[i].ProxyId);
}
}
private void RegisterFixture()
{
// Reserve proxy space
Proxies = new FixtureProxy[Shape.ChildCount];
ProxyCount = 0;
FixtureId = _fixtureIdCounter++;
if ((Body.Flags & BodyFlags.Enabled) == BodyFlags.Enabled)
{
IBroadPhase broadPhase = Body.World.ContactManager.BroadPhase;
CreateProxies(broadPhase, ref Body.Xf);
}
Body.FixtureList.Add(this);
// Adjust mass properties if needed.
if (Shape._density > 0.0f)
{
Body.ResetMassData();
}
// Let the world know we have a new fixture. This will cause new contacts
// to be created at the beginning of the next time step.
Body.World.Flags |= WorldFlags.NewFixture;
if (Body.World.FixtureAdded != null)
{
Body.World.FixtureAdded(this);
}
}
/// <summary>
/// Test a point for containment in this fixture.
/// </summary>
/// <param name="point">A point in world coordinates.</param>
/// <returns></returns>
public bool TestPoint(ref Vector2 point)
{
return Shape.TestPoint(ref Body.Xf, ref point);
}
/// <summary>
/// Cast a ray against this Shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="childIndex">Index of the child.</param>
/// <returns></returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
{
return Shape.RayCast(out output, ref input, ref Body.Xf, childIndex);
}
/// <summary>
/// Get the fixture's AABB. This AABB may be enlarge and/or stale.
/// If you need a more accurate AABB, compute it using the Shape and
/// the body transform.
/// </summary>
/// <param name="aabb">The aabb.</param>
/// <param name="childIndex">Index of the child.</param>
public void GetAABB(out AABB aabb, int childIndex)
{
Debug.Assert(0 <= childIndex && childIndex < ProxyCount);
aabb = Proxies[childIndex].AABB;
}
public Fixture Clone(Body body)
{
Fixture fixture = new Fixture();
fixture.Body = body;
#pragma warning disable 162
if (Settings.ConserveMemory)
fixture.Shape = Shape;
else
fixture.Shape = Shape.Clone();
#pragma warning restore 162
fixture.UserData = UserData;
fixture.Restitution = Restitution;
fixture.Friction = Friction;
fixture.IsSensor = IsSensor;
fixture._collisionGroup = CollisionGroup;
fixture._collisionCategories = CollisionCategories;
fixture._collidesWith = CollidesWith;
if (_collisionIgnores != null)
{
fixture._collisionIgnores = new Dictionary<int, bool>();
foreach (KeyValuePair<int, bool> pair in _collisionIgnores)
{
fixture._collisionIgnores.Add(pair.Key, pair.Value);
}
}
fixture.RegisterFixture();
return fixture;
}
public Fixture DeepClone()
{
Fixture fix = Clone(Body.Clone());
return fix;
}
internal void Destroy()
{
// The proxies must be destroyed before calling this.
Debug.Assert(ProxyCount == 0);
// Free the proxy array.
Proxies = null;
Shape = null;
BeforeCollision = null;
OnCollision = null;
OnSeparation = null;
AfterCollision = null;
if (Body.World.FixtureRemoved != null)
{
Body.World.FixtureRemoved(this);
}
Body.World.FixtureAdded = null;
Body.World.FixtureRemoved = null;
OnSeparation = null;
OnCollision = null;
}
// These support body activation/deactivation.
internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf)
{
Debug.Assert(ProxyCount == 0);
// Create proxies in the broad-phase.
ProxyCount = Shape.ChildCount;
for (int i = 0; i < ProxyCount; ++i)
{
FixtureProxy proxy = new FixtureProxy();
Shape.ComputeAABB(out proxy.AABB, ref xf, i);
proxy.Fixture = this;
proxy.ChildIndex = i;
proxy.ProxyId = broadPhase.AddProxy(ref proxy);
Proxies[i] = proxy;
}
}
internal void DestroyProxies(IBroadPhase broadPhase)
{
// Destroy proxies in the broad-phase.
for (int i = 0; i < ProxyCount; ++i)
{
broadPhase.RemoveProxy(Proxies[i].ProxyId);
Proxies[i].ProxyId = -1;
}
ProxyCount = 0;
}
internal void Synchronize(IBroadPhase broadPhase, ref Transform transform1, ref Transform transform2)
{
if (ProxyCount == 0)
{
return;
}
for (int i = 0; i < ProxyCount; ++i)
{
FixtureProxy proxy = Proxies[i];
// Compute an AABB that covers the swept Shape (may miss some rotation effect).
AABB aabb1, aabb2;
Shape.ComputeAABB(out aabb1, ref transform1, proxy.ChildIndex);
Shape.ComputeAABB(out aabb2, ref transform2, proxy.ChildIndex);
proxy.AABB.Combine(ref aabb1, ref aabb2);
Vector2 displacement = transform2.Position - transform1.Position;
broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement);
}
}
internal bool CompareTo(Fixture fixture)
{
return (
CollidesWith == fixture.CollidesWith &&
CollisionCategories == fixture.CollisionCategories &&
CollisionGroup == fixture.CollisionGroup &&
Friction == fixture.Friction &&
IsSensor == fixture.IsSensor &&
Restitution == fixture.Restitution &&
Shape.CompareTo(fixture.Shape) &&
UserData == fixture.UserData);
}
}
}

484
axios/Dynamics/Island.cs Normal file
View File

@@ -0,0 +1,484 @@
/*
* 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.Common;
using FarseerPhysics.Dynamics.Contacts;
using FarseerPhysics.Dynamics.Joints;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics
{
/// <summary>
/// This is an internal class.
/// </summary>
public class Island
{
public Body[] Bodies;
public int BodyCount;
public int ContactCount;
public int JointCount;
private int _bodyCapacity;
private int _contactCapacity;
private ContactManager _contactManager;
private ContactSolver _contactSolver = new ContactSolver();
private Contact[] _contacts;
private int _jointCapacity;
private Joint[] _joints;
public float JointUpdateTime;
private const float LinTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
private const float AngTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#if (!SILVERLIGHT)
private Stopwatch _watch = new Stopwatch();
#endif
public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, ContactManager contactManager)
{
_bodyCapacity = bodyCapacity;
_contactCapacity = contactCapacity;
_jointCapacity = jointCapacity;
BodyCount = 0;
ContactCount = 0;
JointCount = 0;
_contactManager = contactManager;
if (Bodies == null || Bodies.Length < bodyCapacity)
{
Bodies = new Body[bodyCapacity];
}
if (_contacts == null || _contacts.Length < contactCapacity)
{
_contacts = new Contact[contactCapacity * 2];
}
if (_joints == null || _joints.Length < jointCapacity)
{
_joints = new Joint[jointCapacity * 2];
}
}
public void Clear()
{
BodyCount = 0;
ContactCount = 0;
JointCount = 0;
}
private float _tmpTime;
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType != BodyType.Dynamic)
{
continue;
}
// Integrate velocities.
// FPE 3 only - Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
else
{
b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int i1 = -1;
for (int i2 = 0; i2 < ContactCount; ++i2)
{
Fixture fixtureA = _contacts[i2].FixtureA;
Fixture fixtureB = _contacts[i2].FixtureB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static;
if (nonStatic)
{
++i1;
//TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162
Contact tmp = _contacts[i1];
_contacts[i1] = _contacts[i2];
_contacts[i2] = tmp;
}
}
// Initialize velocity constraints.
_contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
_tmpTime = 0;
}
#endif
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
_joints[i].InitVelocityConstraints(ref step);
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_tmpTime += _watch.ElapsedTicks;
}
#endif
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
_watch.Start();
#endif
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
continue;
joint.SolveVelocityConstraints(ref step);
joint.Validate(step.inv_dt);
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
_contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
_contactSolver.StoreImpulses();
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationX = step.dt * b.LinearVelocityInternal.X;
float translationY = step.dt * b.LinearVelocityInternal.Y;
float result = translationX * translationX + translationY * translationY;
if (result > Settings.MaxTranslationSquared)
{
float sq = (float)Math.Sqrt(result);
float ratio = Settings.MaxTranslation / sq;
b.LinearVelocityInternal.X *= ratio;
b.LinearVelocityInternal.Y *= ratio;
}
float rotation = step.dt * b.AngularVelocityInternal;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
b.AngularVelocityInternal *= ratio;
}
// Store positions for continuous collision.
b.Sweep.C0.X = b.Sweep.C.X;
b.Sweep.C0.Y = b.Sweep.C.Y;
b.Sweep.A0 = b.Sweep.A;
// Integrate
b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X;
b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y;
b.Sweep.A += step.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
_watch.Start();
#endif
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
continue;
bool jointOkay = joint.SolvePositionConstraints();
jointsOkay = jointsOkay && jointOkay;
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
JointUpdateTime = _tmpTime;
}
#endif
Report(_contactSolver.Constraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += step.dt;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
internal void SolveTOI(ref TimeStep subStep)
{
_contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio, false);
// Solve position constraints.
const float kTOIBaumgarte = 0.75f;
for (int i = 0; i < Settings.TOIPositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(kTOIBaumgarte);
if (contactsOkay)
{
break;
}
if (i == Settings.TOIPositionIterations - 1)
{
i += 0;
}
}
// Leap of faith to new safe state.
for (int i = 0; i < BodyCount; ++i)
{
Body body = Bodies[i];
body.Sweep.A0 = body.Sweep.A;
body.Sweep.C0 = body.Sweep.C;
}
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
_contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < Settings.TOIVelocityIterations; ++i)
{
_contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationx = subStep.dt * b.LinearVelocityInternal.X;
float translationy = subStep.dt * b.LinearVelocityInternal.Y;
float dot = translationx * translationx + translationy * translationy;
if (dot > Settings.MaxTranslationSquared)
{
float norm = 1f / (float)Math.Sqrt(dot);
float value = Settings.MaxTranslation * subStep.inv_dt;
b.LinearVelocityInternal.X = value * (translationx * norm);
b.LinearVelocityInternal.Y = value * (translationy * norm);
}
float rotation = subStep.dt * b.AngularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation;
}
else
{
b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation;
}
}
// Integrate
b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X;
b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y;
b.Sweep.A += subStep.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
Report(_contactSolver.Constraints);
}
public void Add(Body body)
{
Debug.Assert(BodyCount < _bodyCapacity);
Bodies[BodyCount++] = body;
}
public void Add(Contact contact)
{
Debug.Assert(ContactCount < _contactCapacity);
_contacts[ContactCount++] = contact;
}
public void Add(Joint joint)
{
Debug.Assert(JointCount < _jointCapacity);
_joints[JointCount++] = joint;
}
private void Report(ContactConstraint[] constraints)
{
if (_contactManager == null)
return;
for (int i = 0; i < ContactCount; ++i)
{
Contact c = _contacts[i];
if (c.FixtureA.AfterCollision != null)
c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c);
if (c.FixtureB.AfterCollision != null)
c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c);
if (_contactManager.PostSolve != null)
{
ContactConstraint cc = constraints[i];
_contactManager.PostSolve(c, cc);
}
}
}
}
}

93
axios/Dynamics/Joints/AngleJoint.cs Normal file
View File

@@ -0,0 +1,93 @@
using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// Maintains a fixed angle between two bodies
/// </summary>
public class AngleJoint : Joint
{
public float BiasFactor;
public float MaxImpulse;
public float Softness;
private float _bias;
private float _jointError;
private float _massFactor;
private float _targetAngle;
internal AngleJoint()
{
JointType = JointType.Angle;
}
public AngleJoint(Body bodyA, Body bodyB)
: base(bodyA, bodyB)
{
JointType = JointType.Angle;
TargetAngle = 0;
BiasFactor = .2f;
Softness = 0f;
MaxImpulse = float.MaxValue;
}
public float TargetAngle
{
get { return _targetAngle; }
set
{
if (value != _targetAngle)
{
_targetAngle = value;
WakeBodies();
}
}
}
public override Vector2 WorldAnchorA
{
get { return BodyA.Position; }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.Position; }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 GetReactionForce(float inv_dt)
{
//TODO
//return _inv_dt * _impulse;
return Vector2.Zero;
}
public override float GetReactionTorque(float inv_dt)
{
return 0;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
_jointError = (BodyB.Sweep.A - BodyA.Sweep.A - TargetAngle);
_bias = -BiasFactor * step.inv_dt * _jointError;
_massFactor = (1 - Softness) / (BodyA.InvI + BodyB.InvI);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
float p = (_bias - BodyB.AngularVelocity + BodyA.AngularVelocity) * _massFactor;
BodyA.AngularVelocity -= BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
BodyB.AngularVelocity += BodyB.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
}
internal override bool SolvePositionConstraints()
{
//no position solving for this joint
return true;
}
}
}

286
axios/Dynamics/Joints/DistanceJoint.cs Normal file
View File

@@ -0,0 +1,286 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// 1-D rained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
/// <summary>
/// A distance joint rains two points on two bodies
/// to remain at a fixed distance from each other. You can view
/// this as a massless, rigid rod.
/// </summary>
public class DistanceJoint : Joint
{
/// <summary>
/// The local anchor point relative to bodyA's origin.
/// </summary>
public Vector2 LocalAnchorA;
/// <summary>
/// The local anchor point relative to bodyB's origin.
/// </summary>
public Vector2 LocalAnchorB;
private float _bias;
private float _gamma;
private float _impulse;
private float _mass;
private float _tmpFloat1;
private Vector2 _tmpVector1;
private Vector2 _u;
internal DistanceJoint()
{
JointType = JointType.Distance;
}
/// <summary>
/// This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. If you don't supply a length, the local anchor points
/// is used so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// @warning Do not use a zero or short length.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="localAnchorA">The first body anchor</param>
/// <param name="localAnchorB">The second body anchor</param>
public DistanceJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Distance;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
Vector2 d = WorldAnchorB - WorldAnchorA;
Length = d.Length();
}
/// <summary>
/// The natural length between the anchor points.
/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
/// </summary>
public float Length { get; set; }
/// <summary>
/// The mass-spring-damper frequency in Hertz.
/// </summary>
public float Frequency { get; set; }
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
public float DampingRatio { get; set; }
public override sealed Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override sealed Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 GetReactionForce(float inv_dt)
{
Vector2 F = (inv_dt * _impulse) * _u;
return F;
}
public override float GetReactionTorque(float inv_dt)
{
return 0.0f;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
_u = b2.Sweep.C + r2 - b1.Sweep.C - r1;
// Handle singularity.
float length = _u.Length();
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float cr1u, cr2u;
MathUtils.Cross(ref r1, ref _u, out cr1u);
MathUtils.Cross(ref r2, ref _u, out cr2u);
float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u;
Debug.Assert(invMass > Settings.Epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Frequency > 0.0f)
{
float C = length - Length;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _mass * DampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
b1.AngularVelocityInternal -= b1.InvI * /* r1 x P */ _tmpFloat1;
b2.LinearVelocityInternal += b2.InvMass * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
b2.AngularVelocityInternal += b2.InvI * /* r2 x P */ _tmpFloat1;
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
// Cdot = dot(u, v + cross(w, r))
MathUtils.Cross(b1.AngularVelocityInternal, ref r1, out _tmpVector1);
Vector2 v1 = b1.LinearVelocityInternal + _tmpVector1;
MathUtils.Cross(b2.AngularVelocityInternal, ref r2, out _tmpVector1);
Vector2 v2 = b2.LinearVelocityInternal + _tmpVector1;
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
b1.AngularVelocityInternal -= b1.InvI * _tmpFloat1;
b2.LinearVelocityInternal += b2.InvMass * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
b2.AngularVelocityInternal += b2.InvI * _tmpFloat1;
}
internal override bool SolvePositionConstraints()
{
if (Frequency > 0.0f)
{
// There is no position correction for soft distance constraints.
return true;
}
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
float length = d.Length();
if (length == 0.0f)
return true;
d /= length;
float C = length - Length;
C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
_u = d;
Vector2 P = impulse * _u;
b1.Sweep.C -= b1.InvMass * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
b1.Sweep.A -= b1.InvI * _tmpFloat1;
b2.Sweep.C += b2.InvMass * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
b2.Sweep.A += b2.InvI * _tmpFloat1;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return Math.Abs(C) < Settings.LinearSlop;
}
}
}

84
axios/Dynamics/Joints/FixedAngleJoint.cs Normal file
View File

@@ -0,0 +1,84 @@
using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
public class FixedAngleJoint : Joint
{
public float BiasFactor;
public float MaxImpulse;
public float Softness;
private float _bias;
private float _jointError;
private float _massFactor;
private float _targetAngle;
public FixedAngleJoint(Body bodyA)
: base(bodyA)
{
JointType = JointType.FixedAngle;
TargetAngle = 0;
BiasFactor = .2f;
Softness = 0f;
MaxImpulse = float.MaxValue;
}
public float TargetAngle
{
get { return _targetAngle; }
set
{
if (value != _targetAngle)
{
_targetAngle = value;
WakeBodies();
}
}
}
public override Vector2 WorldAnchorA
{
get { return BodyA.Position; }
}
public override Vector2 WorldAnchorB
{
get { return BodyA.Position; }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 GetReactionForce(float inv_dt)
{
//TODO
//return _inv_dt * _impulse;
return Vector2.Zero;
}
public override float GetReactionTorque(float inv_dt)
{
return 0;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
_jointError = BodyA.Sweep.A - TargetAngle;
_bias = -BiasFactor * step.inv_dt * _jointError;
_massFactor = (1 - Softness) / (BodyA.InvI);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
float p = (_bias - BodyA.AngularVelocity) * _massFactor;
BodyA.AngularVelocity += BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
}
internal override bool SolvePositionConstraints()
{
//no position solving for this joint
return true;
}
}
}

255
axios/Dynamics/Joints/FixedDistanceJoint.cs Normal file
View File

@@ -0,0 +1,255 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// 1-D rained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
/// <summary>
/// A distance joint rains two points on two bodies
/// to remain at a fixed distance from each other. You can view
/// this as a massless, rigid rod.
/// </summary>
public class FixedDistanceJoint : Joint
{
/// <summary>
/// The local anchor point relative to bodyA's origin.
/// </summary>
public Vector2 LocalAnchorA;
private float _bias;
private float _gamma;
private float _impulse;
private float _mass;
private Vector2 _u;
private Vector2 _worldAnchorB;
/// <summary>
/// This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. If you don't supply a length, the local anchor points
/// is used so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// @warning Do not use a zero or short length.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="bodyAnchor">The body anchor.</param>
/// <param name="worldAnchor">The world anchor.</param>
public FixedDistanceJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor)
: base(body)
{
JointType = JointType.FixedDistance;
LocalAnchorA = bodyAnchor;
_worldAnchorB = worldAnchor;
//Calculate the length
Vector2 d = WorldAnchorB - WorldAnchorA;
Length = d.Length();
}
/// <summary>
/// The natural length between the anchor points.
/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
/// </summary>
public float Length { get; set; }
/// <summary>
/// The mass-spring-damper frequency in Hertz.
/// </summary>
public float Frequency { get; set; }
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
public float DampingRatio { get; set; }
public override sealed Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override sealed Vector2 WorldAnchorB
{
get { return _worldAnchorB; }
set { _worldAnchorB = value; }
}
public override Vector2 GetReactionForce(float invDt)
{
return (invDt * _impulse) * _u;
}
public override float GetReactionTorque(float invDt)
{
return 0.0f;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Transform xf1;
b1.GetTransform(out xf1);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchorB;
_u = r2 - b1.Sweep.C - r1;
// Handle singularity.
float length = _u.Length();
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + 0 * cr2u * cr2u;
Debug.Assert(invMass > Settings.Epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Frequency > 0.0f)
{
float C = length - Length;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _mass * DampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
// Cdot = dot(u, v + cross(w, r))
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
Vector2 v2 = Vector2.Zero;
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
}
internal override bool SolvePositionConstraints()
{
if (Frequency > 0.0f)
{
// There is no position correction for soft distance constraints.
return true;
}
Body b1 = BodyA;
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchorB;
Vector2 d = r2 - b1.Sweep.C - r1;
float length = d.Length();
if (length == 0.0f)
return true;
d /= length;
float C = length - Length;
C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
_u = d;
Vector2 P = impulse * _u;
b1.Sweep.C -= b1.InvMass * P;
b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P);
b1.SynchronizeTransform();
return Math.Abs(C) < Settings.LinearSlop;
}
}
}

227
axios/Dynamics/Joints/FixedFrictionJoint.cs Normal file
View File

@@ -0,0 +1,227 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Point-to-point constraint
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
/// <summary>
/// Friction joint. This is used for top-down friction.
/// It provides 2D translational friction and angular friction.
/// </summary>
public class FixedFrictionJoint : Joint
{
public Vector2 LocalAnchorA;
/// <summary>
/// The maximum friction force in N.
/// </summary>
public float MaxForce;
/// <summary>
/// The maximum friction torque in N-m.
/// </summary>
public float MaxTorque;
private float _angularImpulse;
private float _angularMass;
private Vector2 _linearImpulse;
private Mat22 _linearMass;
public FixedFrictionJoint(Body body, Vector2 localAnchorA)
: base(body)
{
JointType = JointType.FixedFriction;
LocalAnchorA = localAnchorA;
//Setting default max force and max torque
const float gravity = 10.0f;
// For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m)
float radius = (float)Math.Sqrt(2.0 * (body.Inertia / body.Mass));
MaxForce = body.Mass * gravity;
MaxTorque = body.Mass * radius * gravity;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return Vector2.Zero; }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 GetReactionForce(float invDT)
{
return invDT * _linearImpulse;
}
public override float GetReactionTorque(float invDT)
{
return invDT * _angularImpulse;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Transform xfA;
bA.GetTransform(out xfA);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA.InvMass;
float iA = bA.InvI;
Mat22 K1 = new Mat22();
K1.Col1.X = mA;
K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f;
K1.Col2.Y = mA;
Mat22 K2 = new Mat22();
K2.Col1.X = iA * rA.Y * rA.Y;
K2.Col2.X = -iA * rA.X * rA.Y;
K2.Col1.Y = -iA * rA.X * rA.Y;
K2.Col2.Y = iA * rA.X * rA.X;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
_linearMass = K12.Inverse;
_angularMass = iA;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Vector2 vA = bA.LinearVelocityInternal;
float wA = bA.AngularVelocityInternal;
float mA = bA.InvMass;
float iA = bA.InvI;
Transform xfA;
bA.GetTransform(out xfA);
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
// Solve angular friction
{
float Cdot = -wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = step.dt * MaxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
}
// Solve linear friction
{
Vector2 Cdot = -vA - MathUtils.Cross(wA, rA);
Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = step.dt * MaxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(rA, impulse);
}
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
}
internal override bool SolvePositionConstraints()
{
return true;
}
}
}

413
axios/Dynamics/Joints/FixedLineJoint.cs Normal file
View File

@@ -0,0 +1,413 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
public class FixedLineJoint : Joint
{
private Vector2 _ax, _ay;
private float _bias;
private bool _enableMotor;
private float _gamma;
private float _impulse;
private Vector2 _localXAxis;
private Vector2 _localYAxisA;
private float _mass;
private float _maxMotorTorque;
private float _motorImpulse;
private float _motorMass;
private float _motorSpeed;
private float _sAx;
private float _sAy;
private float _sBx;
private float _sBy;
private float _springImpulse;
private float _springMass;
// Linear constraint (point-to-line)
// d = pB - pA = xB + rB - xA - rA
// C = dot(ay, d)
// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
// Spring linear constraint
// C = dot(ax, d)
// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
// Motor rotational constraint
// Cdot = wB - wA
// J = [0 0 -1 0 0 1]
internal FixedLineJoint() { JointType = JointType.FixedLine; }
public FixedLineJoint(Body body, Vector2 worldAnchor, Vector2 axis)
: base(body)
{
JointType = JointType.FixedLine;
BodyB = BodyA;
LocalAnchorA = worldAnchor;
LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
LocalXAxis = axis;
}
public Vector2 LocalAnchorA { get; set; }
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get { return LocalAnchorA; }
}
public override Vector2 WorldAnchorB
{
get { return BodyA.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public float JointTranslation
{
get
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
Vector2 d = pB - pA;
Vector2 axis = bA.GetWorldVector(LocalXAxis);
float translation = Vector2.Dot(d, axis);
return translation;
}
}
public float JointSpeed
{
get
{
float wA = BodyA.AngularVelocityInternal;
float wB = BodyB.AngularVelocityInternal;
return wB - wA;
}
}
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
BodyA.Awake = true;
BodyB.Awake = true;
_enableMotor = value;
}
}
public float MotorSpeed
{
set
{
BodyA.Awake = true;
BodyB.Awake = true;
_motorSpeed = value;
}
get { return _motorSpeed; }
}
public float MaxMotorTorque
{
set
{
BodyA.Awake = true;
BodyB.Awake = true;
_maxMotorTorque = value;
}
get { return _maxMotorTorque; }
}
public float Frequency { get; set; }
public float DampingRatio { get; set; }
public Vector2 LocalXAxis
{
get { return _localXAxis; }
set
{
_localXAxis = value;
_localYAxisA = MathUtils.Cross(1.0f, _localXAxis);
}
}
public override Vector2 GetReactionForce(float invDt)
{
return invDt * (_impulse * _ay + _springImpulse * _ax);
}
public override float GetReactionTorque(float invDt)
{
return invDt * _motorImpulse;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
LocalCenterA = Vector2.Zero;
LocalCenterB = bB.LocalCenter;
Transform xfB;
bB.GetTransform(out xfB);
// Compute the effective masses.
Vector2 rA = LocalAnchorA;
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB);
Vector2 d = bB.Sweep.C + rB - rA;
InvMassA = 0.0f;
InvIA = 0.0f;
InvMassB = bB.InvMass;
InvIB = bB.InvI;
// Point to line constraint
{
_ay = _localYAxisA;
_sAy = MathUtils.Cross(d + rA, _ay);
_sBy = MathUtils.Cross(rB, _ay);
_mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
if (_mass > 0.0f)
{
_mass = 1.0f / _mass;
}
}
// Spring constraint
_springMass = 0.0f;
if (Frequency > 0.0f)
{
_ax = LocalXAxis;
_sAx = MathUtils.Cross(d + rA, _ax);
_sBx = MathUtils.Cross(rB, _ax);
float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx;
if (invMass > 0.0f)
{
_springMass = 1.0f / invMass;
float C = Vector2.Dot(d, _ax);
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float da = 2.0f * _springMass * DampingRatio * omega;
// Spring stiffness
float k = _springMass * omega * omega;
// magic formulas
_gamma = step.dt * (da + step.dt * k);
if (_gamma > 0.0f)
{
_gamma = 1.0f / _gamma;
}
_bias = C * step.dt * k * _gamma;
_springMass = invMass + _gamma;
if (_springMass > 0.0f)
{
_springMass = 1.0f / _springMass;
}
}
}
else
{
_springImpulse = 0.0f;
_springMass = 0.0f;
}
// Rotational motor
if (_enableMotor)
{
_motorMass = InvIA + InvIB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
else
{
_motorMass = 0.0f;
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_springImpulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse * _ay + _springImpulse * _ax;
float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
bB.LinearVelocityInternal += InvMassB * P;
bB.AngularVelocityInternal += InvIB * LB;
}
else
{
_impulse = 0.0f;
_springImpulse = 0.0f;
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
Vector2 vA = Vector2.Zero;
float wA = 0.0f;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
// Solve spring constraint
{
float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
_springImpulse += impulse;
Vector2 P = impulse * _ax;
float LA = impulse * _sAx;
float LB = impulse * _sBx;
vA -= InvMassA * P;
wA -= InvIA * LA;
vB += InvMassB * P;
wB += InvIB * LB;
}
// Solve rotational motor constraint
{
float Cdot = wB - wA - _motorSpeed;
float impulse = -_motorMass * Cdot;
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= InvIA * impulse;
wB += InvIB * impulse;
}
// Solve point to line constraint
{
float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
float impulse = _mass * (-Cdot);
_impulse += impulse;
Vector2 P = impulse * _ay;
float LB = impulse * _sBy;
vB += InvMassB * P;
wB += InvIB * LB;
}
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
internal override bool SolvePositionConstraints()
{
Body bB = BodyB;
Vector2 xA = Vector2.Zero;
const float angleA = 0.0f;
Vector2 xB = bB.Sweep.C;
float angleB = bB.Sweep.A;
Mat22 RA = new Mat22(angleA);
Mat22 RB = new Mat22(angleB);
Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA);
Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB);
Vector2 d = xB + rB - xA - rA;
Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA);
float sBy = MathUtils.Cross(rB, ay);
float C = Vector2.Dot(d, ay);
float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
float impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
Vector2 P = impulse * ay;
float LB = impulse * sBy;
xB += InvMassB * P;
angleB += InvIB * LB;
// TODO_ERIN remove need for this.
bB.Sweep.C = xB;
bB.Sweep.A = angleB;
bB.SynchronizeTransform();
return Math.Abs(C) <= Settings.LinearSlop;
}
public float GetMotorTorque(float invDt)
{
return invDt * _motorImpulse;
}
}
}

209
axios/Dynamics/Joints/FixedMouseJoint.cs Normal file
View File

@@ -0,0 +1,209 @@
/*
* 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.Diagnostics;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// A mouse joint is used to make a point on a body track a
/// specified world point. This a soft constraint with a maximum
/// force. This allows the constraint to stretch and without
/// applying huge forces.
/// NOTE: this joint is not documented in the manual because it was
/// developed to be used in the testbed. If you want to learn how to
/// use the mouse joint, look at the testbed.
/// </summary>
public class FixedMouseJoint : Joint
{
public Vector2 LocalAnchorA;
private Vector2 _C; // position error
private float _beta;
private float _gamma;
private Vector2 _impulse;
private Mat22 _mass; // effective mass for point-to-point constraint.
private Vector2 _worldAnchor;
/// <summary>
/// This requires a world target point,
/// tuning parameters, and the time step.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The target.</param>
public FixedMouseJoint(Body body, Vector2 worldAnchor)
: base(body)
{
JointType = JointType.FixedMouse;
Frequency = 5.0f;
DampingRatio = 0.7f;
Debug.Assert(worldAnchor.IsValid());
Transform xf1;
BodyA.GetTransform(out xf1);
_worldAnchor = worldAnchor;
LocalAnchorA = BodyA.GetLocalPoint(worldAnchor);
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return _worldAnchor; }
set
{
BodyA.Awake = true;
_worldAnchor = value;
}
}
/// <summary>
/// The maximum constraint force that can be exerted
/// to move the candidate body. Usually you will express
/// as some multiple of the weight (multiplier * mass * gravity).
/// </summary>
public float MaxForce { get; set; }
/// <summary>
/// The response speed.
/// </summary>
public float Frequency { get; set; }
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
public float DampingRatio { get; set; }
public override Vector2 GetReactionForce(float inv_dt)
{
return inv_dt * _impulse;
}
public override float GetReactionTorque(float inv_dt)
{
return inv_dt * 0.0f;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = BodyA;
float mass = b.Mass;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Debug.Assert(d + step.dt * k > Settings.Epsilon);
_gamma = step.dt * (d + step.dt * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = step.dt * k * _gamma;
// Compute the effective mass matrix.
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
float invMass = b.InvMass;
float invI = b.InvI;
Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y),
new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));
Mat22 K;
Mat22.Add(ref K1, ref K2, out K);
K.Col1.X += _gamma;
K.Col2.Y += _gamma;
_mass = K.Inverse;
_C = b.Sweep.C + r - _worldAnchor;
// Cheat with some damping
b.AngularVelocityInternal *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b.LinearVelocityInternal += invMass * _impulse;
b.AngularVelocityInternal += invI * MathUtils.Cross(r, _impulse);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b = BodyA;
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
// Cdot = v + cross(w, r)
Vector2 Cdot = b.LinearVelocityInternal + MathUtils.Cross(b.AngularVelocityInternal, r);
Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse));
Vector2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = step.dt * MaxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
b.LinearVelocityInternal += b.InvMass * impulse;
b.AngularVelocityInternal += b.InvI * MathUtils.Cross(r, impulse);
}
internal override bool SolvePositionConstraints()
{
return true;
}
}
}

636
axios/Dynamics/Joints/FixedPrismaticJoint.cs Normal file
View File

@@ -0,0 +1,636 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Linear constraint (point-to-line)
// d = p2 - p1 = x2 + r2 - x1 - r1
// C = dot(perp, d)
// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
//
// Angular constraint
// C = a2 - a1 + a_initial
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
//
// K = J * invM * JT
//
// J = [-a -s1 a s2]
// [0 -1 0 1]
// a = perp
// s1 = cross(d + r1, a) = cross(p2 - x1, a)
// s2 = cross(r2, a) = cross(p2 - x2, a)
// Motor/Limit linear constraint
// C = dot(ax1, d)
// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
// Block Solver
// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
// when the mass has poor distribution (leading to large torques about the joint anchor points).
//
// The Jacobian has 3 rows:
// J = [-uT -s1 uT s2] // linear
// [0 -1 0 1] // angular
// [-vT -a1 vT a2] // limit
//
// u = perp
// v = axis
// s1 = cross(d + r1, u), s2 = cross(r2, u)
// a1 = cross(d + r1, v), a2 = cross(r2, v)
// M * (v2 - v1) = JT * df
// J * v2 = bias
//
// v2 = v1 + invM * JT * df
// J * (v1 + invM * JT * df) = bias
// K * df = bias - J * v1 = -Cdot
// K = J * invM * JT
// Cdot = J * v1 - bias
//
// Now solve for f2.
// df = f2 - f1
// K * (f2 - f1) = -Cdot
// f2 = invK * (-Cdot) + f1
//
// Clamp accumulated limit impulse.
// lower: f2(3) = max(f2(3), 0)
// upper: f2(3) = min(f2(3), 0)
//
// Solve for correct f2(1:2)
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
//
// Now compute impulse to be applied:
// df = f2 - f1
/// <summary>
/// A prismatic joint. This joint provides one degree of freedom: translation
/// along an axis fixed in body1. Relative rotation is prevented. You can
/// use a joint limit to restrict the range of motion and a joint motor to
/// drive the motion or to model joint friction.
/// </summary>
public class FixedPrismaticJoint : Joint
{
private Mat33 _K;
private float _a1, _a2;
private Vector2 _axis;
private bool _enableLimit;
private bool _enableMotor;
private Vector3 _impulse;
private LimitState _limitState;
private Vector2 _localXAxis1;
private Vector2 _localYAxis1;
private float _lowerTranslation;
private float _maxMotorForce;
private float _motorMass; // effective mass for motor/limit translational constraint.
private float _motorSpeed;
private Vector2 _perp;
private float _refAngle;
private float _s1, _s2;
private float _upperTranslation;
/// <summary>
/// This requires defining a line of
/// motion using an axis and an anchor point. The definition uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The anchor.</param>
/// <param name="axis">The axis.</param>
public FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis)
: base(body)
{
JointType = JointType.FixedPrismatic;
BodyB = BodyA;
LocalAnchorA = worldAnchor;
LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
_localXAxis1 = axis;
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_refAngle = BodyB.Rotation;
_limitState = LimitState.Inactive;
}
public Vector2 LocalAnchorA { get; set; }
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get { return LocalAnchorA; }
}
public override Vector2 WorldAnchorB
{
get { return BodyA.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// Get the current joint translation, usually in meters.
/// </summary>
/// <value></value>
public float JointTranslation
{
get
{
Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - LocalAnchorA;
Vector2 axis = _localXAxis1;
return Vector2.Dot(d, axis);
}
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
/// <value></value>
public float JointSpeed
{
get
{
Transform xf2;
BodyB.GetTransform(out xf2);
Vector2 r1 = LocalAnchorA;
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter);
Vector2 p1 = r1;
Vector2 p2 = BodyB.Sweep.C + r2;
Vector2 d = p2 - p1;
Vector2 axis = _localXAxis1;
Vector2 v1 = Vector2.Zero;
Vector2 v2 = BodyB.LinearVelocityInternal;
const float w1 = 0.0f;
float w2 = BodyB.AngularVelocityInternal;
float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) +
Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
return speed;
}
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public bool LimitEnabled
{
get { return _enableLimit; }
set
{
Debug.Assert(BodyA.FixedRotation == false, "Warning: limits does currently not work with fixed rotation");
WakeBodies();
_enableLimit = value;
}
}
/// <summary>
/// Get the lower joint limit, usually in meters.
/// </summary>
/// <value></value>
public float LowerLimit
{
get { return _lowerTranslation; }
set
{
WakeBodies();
_lowerTranslation = value;
}
}
/// <summary>
/// Get the upper joint limit, usually in meters.
/// </summary>
/// <value></value>
public float UpperLimit
{
get { return _upperTranslation; }
set
{
WakeBodies();
_upperTranslation = value;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Set the motor speed, usually in meters per second.
/// </summary>
/// <value>The speed.</value>
public float MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get { return _motorSpeed; }
}
/// <summary>
/// Set the maximum motor force, usually in N.
/// </summary>
/// <value>The force.</value>
public float MaxMotorForce
{
set
{
WakeBodies();
_maxMotorForce = value;
}
}
/// <summary>
/// Get the current motor force, usually in N.
/// </summary>
/// <value></value>
public float MotorForce { get; set; }
public Vector2 LocalXAxis1
{
get { return _localXAxis1; }
set
{
_localXAxis1 = value;
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
}
}
public override Vector2 GetReactionForce(float inv_dt)
{
return inv_dt * (_impulse.X * _perp + (MotorForce + _impulse.Z) * _axis);
}
public override float GetReactionTorque(float inv_dt)
{
return inv_dt * _impulse.Y;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
LocalCenterA = Vector2.Zero;
LocalCenterB = bB.LocalCenter;
Transform xf2;
bB.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = LocalAnchorA;
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = bB.Sweep.C + r2 - /* b1._sweep.Center - */ r1;
InvMassA = 0.0f;
InvIA = 0.0f;
InvMassB = bB.InvMass;
InvIB = bB.InvI;
// Compute motor Jacobian and effective mass.
{
_axis = _localXAxis1;
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if (_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = _localYAxis1;
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
MotorForce = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
MotorForce *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (MotorForce + _impulse.Z) * _axis;
float L2 = _impulse.X * _s2 + _impulse.Y + (MotorForce + _impulse.Z) * _a2;
bB.LinearVelocityInternal += InvMassB * P;
bB.AngularVelocityInternal += InvIB * L2;
}
else
{
_impulse = Vector3.Zero;
MotorForce = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
Vector2 v1 = Vector2.Zero;
float w1 = 0.0f;
Vector2 v2 = bB.LinearVelocityInternal;
float w2 = bB.AngularVelocityInternal;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = MotorForce;
float maxImpulse = step.dt * _maxMotorForce;
MotorForce = MathUtils.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse);
impulse = MotorForce - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float L2 = df.X * _s2 + df.Y;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
bB.LinearVelocityInternal = v2;
bB.AngularVelocityInternal = w2;
}
internal override bool SolvePositionConstraints()
{
//Body b1 = BodyA;
Body b2 = BodyB;
Vector2 c1 = Vector2.Zero; // b1._sweep.Center;
float a1 = 0.0f; // b1._sweep.Angle;
Vector2 c2 = b2.Sweep.C;
float a2 = b2.Sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
float angularError = Math.Abs(C1.Y);
if (active)
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C); // negated above
}
else
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b2.Sweep.C = c2;
b2.Sweep.A = a2;
b2.SynchronizeTransform();
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}

541
axios/Dynamics/Joints/FixedRevoluteJoint.cs Normal file
View File

@@ -0,0 +1,541 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// A revolute joint rains to bodies to share a common point while they
/// are free to rotate about the point. The relative rotation about the shared
/// point is the joint angle. You can limit the relative rotation with
/// a joint limit that specifies a lower and upper angle. You can use a motor
/// to drive the relative rotation about the shared point. A maximum motor torque
/// is provided so that infinite forces are not generated.
/// </summary>
public class FixedRevoluteJoint : Joint
{
private bool _enableLimit;
private bool _enableMotor;
private Vector3 _impulse;
private LimitState _limitState;
private float _lowerAngle;
private Mat33 _mass; // effective mass for point-to-point constraint.
private float _maxMotorTorque;
private float _motorImpulse;
private float _motorMass; // effective mass for motor/limit angular constraint.
private float _motorSpeed;
private float _upperAngle;
private Vector2 _worldAnchor;
/// <summary>
/// Initialize the bodies, anchors, and reference angle using the world
/// anchor.
/// This requires defining an
/// anchor point where the bodies are joined. The definition
/// uses local anchor points so that the initial configuration
/// can violate the constraint slightly. You also need to
/// specify the initial relative angle for joint limits. This
/// helps when saving and loading a game.
/// The local anchor points are measured from the body's origin
/// rather than the center of mass because:
/// 1. you might not know where the center of mass will be.
/// 2. if you add/remove shapes from a body and recompute the mass,
/// the joints will be broken.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="bodyAnchor">The body anchor.</param>
/// <param name="worldAnchor">The world anchor.</param>
public FixedRevoluteJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor)
: base(body)
{
JointType = JointType.FixedRevolute;
// Changed to local coordinates.
LocalAnchorA = bodyAnchor;
_worldAnchor = worldAnchor;
ReferenceAngle = -BodyA.Rotation;
_impulse = Vector3.Zero;
_limitState = LimitState.Inactive;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return _worldAnchor; }
set { _worldAnchor = value; }
}
public Vector2 LocalAnchorA { get; set; }
public float ReferenceAngle { get; set; }
/// <summary>
/// Get the current joint angle in radians.
/// </summary>
/// <value></value>
public float JointAngle
{
get { return BodyA.Sweep.A - ReferenceAngle; }
}
/// <summary>
/// Get the current joint angle speed in radians per second.
/// </summary>
/// <value></value>
public float JointSpeed
{
get { return BodyA.AngularVelocityInternal; }
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public bool LimitEnabled
{
get { return _enableLimit; }
set
{
WakeBodies();
_enableLimit = value;
}
}
/// <summary>
/// Get the lower joint limit in radians.
/// </summary>
/// <value></value>
public float LowerLimit
{
get { return _lowerAngle; }
set
{
WakeBodies();
_lowerAngle = value;
}
}
/// <summary>
/// Get the upper joint limit in radians.
/// </summary>
/// <value></value>
public float UpperLimit
{
get { return _upperAngle; }
set
{
WakeBodies();
_upperAngle = value;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Set the motor speed in radians per second.
/// </summary>
/// <value>The speed.</value>
public float MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get { return _motorSpeed; }
}
/// <summary>
/// Set the maximum motor torque, usually in N-m.
/// </summary>
/// <value>The torque.</value>
public float MaxMotorTorque
{
set
{
WakeBodies();
_maxMotorTorque = value;
}
get { return _maxMotorTorque; }
}
/// <summary>
/// Get the current motor torque, usually in N-m.
/// </summary>
/// <value></value>
public float MotorTorque
{
get { return _motorImpulse; }
set
{
WakeBodies();
_motorImpulse = value;
}
}
public override Vector2 GetReactionForce(float inv_dt)
{
return inv_dt * new Vector2(_impulse.X, _impulse.Y);
}
public override float GetReactionTorque(float inv_dt)
{
return inv_dt * _impulse.Z;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Debug.Assert(b1.InvI > 0.0f /* || b2._invI > 0.0f*/);
}
// Compute the effective mass matrix.
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor; // MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1.InvMass;
const float m2 = 0;
float i1 = b1.InvI;
const float i2 = 0;
_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.Col3.Y = r1.X * i1 + r2.X * i2;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = i1 + i2;
_motorMass = i1 + i2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = 0 - b1.Sweep.A - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
b1.LinearVelocityInternal -= m1 * P;
b1.AngularVelocityInternal -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = Vector2.Zero;
const float w2 = 0;
float m1 = b1.InvMass;
float i1 = b1.InvI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
v1 -= m1 * P;
w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z);
}
else
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
// Solve point-to-point constraint
Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
w1 -= i1 * MathUtils.Cross(r1, impulse);
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
}
internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = 0 - b1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b1.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
Vector2 C = Vector2.Zero + r2 - b1.Sweep.C - r1;
positionError = C.Length();
float invMass1 = b1.InvMass;
const float invMass2 = 0;
float invI1 = b1.InvI;
const float invI2 = 0;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C;
u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const float k_beta = 0.5f;
b1.Sweep.C -= k_beta * invMass1 * impulse2;
C = Vector2.Zero + r2 - b1.Sweep.C - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
Mat22 Ka;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22 K;
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1.Sweep.C -= b1.InvMass * impulse;
b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse);
b1.SynchronizeTransform();
}
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}

249
axios/Dynamics/Joints/FrictionJoint.cs Normal file
View File

@@ -0,0 +1,249 @@
/*
* 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.Diagnostics;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Point-to-point constraint
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
/// <summary>
/// Friction joint. This is used for top-down friction.
/// It provides 2D translational friction and angular friction.
/// </summary>
public class FrictionJoint : Joint
{
public Vector2 LocalAnchorA;
public Vector2 LocalAnchorB;
private float _angularImpulse;
private float _angularMass;
private Vector2 _linearImpulse;
private Mat22 _linearMass;
internal FrictionJoint()
{
JointType = JointType.Friction;
}
public FrictionJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Friction;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// The maximum friction force in N.
/// </summary>
public float MaxForce { get; set; }
/// <summary>
/// The maximum friction torque in N-m.
/// </summary>
public float MaxTorque { get; set; }
public override Vector2 GetReactionForce(float inv_dt)
{
return inv_dt * _linearImpulse;
}
public override float GetReactionTorque(float inv_dt)
{
return inv_dt * _angularImpulse;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Mat22 K1 = new Mat22();
K1.Col1.X = mA + mB;
K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f;
K1.Col2.Y = mA + mB;
Mat22 K2 = new Mat22();
K2.Col1.X = iA * rA.Y * rA.Y;
K2.Col2.X = -iA * rA.X * rA.Y;
K2.Col1.Y = -iA * rA.X * rA.Y;
K2.Col2.Y = iA * rA.X * rA.X;
Mat22 K3 = new Mat22();
K3.Col1.X = iB * rB.Y * rB.Y;
K3.Col2.X = -iB * rB.X * rB.Y;
K3.Col1.Y = -iB * rB.X * rB.Y;
K3.Col2.Y = iB * rB.X * rB.X;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
Mat22 K;
Mat22.Add(ref K12, ref K3, out K);
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
bB.LinearVelocityInternal += mB * P;
bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 vA = bA.LinearVelocityInternal;
float wA = bA.AngularVelocityInternal;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = step.dt * MaxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = step.dt * MaxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(rB, impulse);
}
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
internal override bool SolvePositionConstraints()
{
return true;
}
}
}

350
axios/Dynamics/Joints/GearJoint.cs Normal file
View File

@@ -0,0 +1,350 @@
/*
* 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.Diagnostics;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// A gear joint is used to connect two joints together. Either joint
/// can be a revolute or prismatic joint. You specify a gear ratio
/// to bind the motions together:
/// coordinate1 + ratio * coordinate2 = ant
/// The ratio can be negative or positive. If one joint is a revolute joint
/// and the other joint is a prismatic joint, then the ratio will have units
/// of length or units of 1/length.
/// @warning The revolute and prismatic joints must be attached to
/// fixed bodies (which must be body1 on those joints).
/// </summary>
public class GearJoint : Joint
{
private Jacobian _J;
private float _ant;
private FixedPrismaticJoint _fixedPrismatic1;
private FixedPrismaticJoint _fixedPrismatic2;
private FixedRevoluteJoint _fixedRevolute1;
private FixedRevoluteJoint _fixedRevolute2;
private float _impulse;
private float _mass;
private PrismaticJoint _prismatic1;
private PrismaticJoint _prismatic2;
private RevoluteJoint _revolute1;
private RevoluteJoint _revolute2;
/// <summary>
/// Requires two existing revolute or prismatic joints (any combination will work).
/// The provided joints must attach a dynamic body to a static body.
/// </summary>
/// <param name="jointA">The first joint.</param>
/// <param name="jointB">The second joint.</param>
/// <param name="ratio">The ratio.</param>
public GearJoint(Joint jointA, Joint jointB, float ratio)
: base(jointA.BodyA, jointA.BodyB)
{
JointType = JointType.Gear;
JointA = jointA;
JointB = jointB;
Ratio = ratio;
JointType type1 = jointA.JointType;
JointType type2 = jointB.JointType;
// Make sure its the right kind of joint
Debug.Assert(type1 == JointType.Revolute ||
type1 == JointType.Prismatic ||
type1 == JointType.FixedRevolute ||
type1 == JointType.FixedPrismatic);
Debug.Assert(type2 == JointType.Revolute ||
type2 == JointType.Prismatic ||
type2 == JointType.FixedRevolute ||
type2 == JointType.FixedPrismatic);
// In the case of a prismatic and revolute joint, the first body must be static.
if (type1 == JointType.Revolute || type1 == JointType.Prismatic)
Debug.Assert(jointA.BodyA.BodyType == BodyType.Static);
if (type2 == JointType.Revolute || type2 == JointType.Prismatic)
Debug.Assert(jointB.BodyA.BodyType == BodyType.Static);
float coordinate1 = 0.0f, coordinate2 = 0.0f;
switch (type1)
{
case JointType.Revolute:
BodyA = jointA.BodyB;
_revolute1 = (RevoluteJoint)jointA;
LocalAnchor1 = _revolute1.LocalAnchorB;
coordinate1 = _revolute1.JointAngle;
break;
case JointType.Prismatic:
BodyA = jointA.BodyB;
_prismatic1 = (PrismaticJoint)jointA;
LocalAnchor1 = _prismatic1.LocalAnchorB;
coordinate1 = _prismatic1.JointTranslation;
break;
case JointType.FixedRevolute:
BodyA = jointA.BodyA;
_fixedRevolute1 = (FixedRevoluteJoint)jointA;
LocalAnchor1 = _fixedRevolute1.LocalAnchorA;
coordinate1 = _fixedRevolute1.JointAngle;
break;
case JointType.FixedPrismatic:
BodyA = jointA.BodyA;
_fixedPrismatic1 = (FixedPrismaticJoint)jointA;
LocalAnchor1 = _fixedPrismatic1.LocalAnchorA;
coordinate1 = _fixedPrismatic1.JointTranslation;
break;
}
switch (type2)
{
case JointType.Revolute:
BodyB = jointB.BodyB;
_revolute2 = (RevoluteJoint)jointB;
LocalAnchor2 = _revolute2.LocalAnchorB;
coordinate2 = _revolute2.JointAngle;
break;
case JointType.Prismatic:
BodyB = jointB.BodyB;
_prismatic2 = (PrismaticJoint)jointB;
LocalAnchor2 = _prismatic2.LocalAnchorB;
coordinate2 = _prismatic2.JointTranslation;
break;
case JointType.FixedRevolute:
BodyB = jointB.BodyA;
_fixedRevolute2 = (FixedRevoluteJoint)jointB;
LocalAnchor2 = _fixedRevolute2.LocalAnchorA;
coordinate2 = _fixedRevolute2.JointAngle;
break;
case JointType.FixedPrismatic:
BodyB = jointB.BodyA;
_fixedPrismatic2 = (FixedPrismaticJoint)jointB;
LocalAnchor2 = _fixedPrismatic2.LocalAnchorA;
coordinate2 = _fixedPrismatic2.JointTranslation;
break;
}
_ant = coordinate1 + Ratio * coordinate2;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchor1); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchor2); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// The gear ratio.
/// </summary>
public float Ratio { get; set; }
/// <summary>
/// The first revolute/prismatic joint attached to the gear joint.
/// </summary>
public Joint JointA { get; set; }
/// <summary>
/// The second revolute/prismatic joint attached to the gear joint.
/// </summary>
public Joint JointB { get; set; }
public Vector2 LocalAnchor1 { get; private set; }
public Vector2 LocalAnchor2 { get; private set; }
public override Vector2 GetReactionForce(float inv_dt)
{
Vector2 P = _impulse * _J.LinearB;
return inv_dt * P;
}
public override float GetReactionTorque(float inv_dt)
{
Transform xf1;
BodyB.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor2 - BodyB.LocalCenter);
Vector2 P = _impulse * _J.LinearB;
float L = _impulse * _J.AngularB - MathUtils.Cross(r, P);
return inv_dt * L;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
float K = 0.0f;
_J.SetZero();
if (_revolute1 != null || _fixedRevolute1 != null)
{
_J.AngularA = -1.0f;
K += b1.InvI;
}
else
{
Vector2 ug;
if (_prismatic1 != null)
ug = _prismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
else
ug = _fixedPrismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
Transform xf1 /*, xfg1*/;
b1.GetTransform(out xf1);
//g1.GetTransform(out xfg1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor1 - b1.LocalCenter);
float crug = MathUtils.Cross(r, ug);
_J.LinearA = -ug;
_J.AngularA = -crug;
K += b1.InvMass + b1.InvI * crug * crug;
}
if (_revolute2 != null || _fixedRevolute2 != null)
{
_J.AngularB = -Ratio;
K += Ratio * Ratio * b2.InvI;
}
else
{
Vector2 ug;
if (_prismatic2 != null)
ug = _prismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
else
ug = _fixedPrismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
Transform /*xfg1,*/ xf2;
//g1.GetTransform(out xfg1);
b2.GetTransform(out xf2);
Vector2 r = MathUtils.Multiply(ref xf2.R, LocalAnchor2 - b2.LocalCenter);
float crug = MathUtils.Cross(r, ug);
_J.LinearB = -Ratio * ug;
_J.AngularB = -Ratio * crug;
K += Ratio * Ratio * (b2.InvMass + b2.InvI * crug * crug);
}
// Compute effective mass.
Debug.Assert(K > 0.0f);
_mass = K > 0.0f ? 1.0f / K : 0.0f;
if (Settings.EnableWarmstarting)
{
// Warm starting.
b1.LinearVelocityInternal += b1.InvMass * _impulse * _J.LinearA;
b1.AngularVelocityInternal += b1.InvI * _impulse * _J.AngularA;
b2.LinearVelocityInternal += b2.InvMass * _impulse * _J.LinearB;
b2.AngularVelocityInternal += b2.InvI * _impulse * _J.AngularB;
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
float Cdot = _J.Compute(b1.LinearVelocityInternal, b1.AngularVelocityInternal,
b2.LinearVelocityInternal, b2.AngularVelocityInternal);
float impulse = _mass * (-Cdot);
_impulse += impulse;
b1.LinearVelocityInternal += b1.InvMass * impulse * _J.LinearA;
b1.AngularVelocityInternal += b1.InvI * impulse * _J.AngularA;
b2.LinearVelocityInternal += b2.InvMass * impulse * _J.LinearB;
b2.AngularVelocityInternal += b2.InvI * impulse * _J.AngularB;
}
internal override bool SolvePositionConstraints()
{
const float linearError = 0.0f;
Body b1 = BodyA;
Body b2 = BodyB;
float coordinate1 = 0.0f, coordinate2 = 0.0f;
if (_revolute1 != null)
{
coordinate1 = _revolute1.JointAngle;
}
else if (_fixedRevolute1 != null)
{
coordinate1 = _fixedRevolute1.JointAngle;
}
else if (_prismatic1 != null)
{
coordinate1 = _prismatic1.JointTranslation;
}
else if (_fixedPrismatic1 != null)
{
coordinate1 = _fixedPrismatic1.JointTranslation;
}
if (_revolute2 != null)
{
coordinate2 = _revolute2.JointAngle;
}
else if (_fixedRevolute2 != null)
{
coordinate2 = _fixedRevolute2.JointAngle;
}
else if (_prismatic2 != null)
{
coordinate2 = _prismatic2.JointTranslation;
}
else if (_fixedPrismatic2 != null)
{
coordinate2 = _fixedPrismatic2.JointTranslation;
}
float C = _ant - (coordinate1 + Ratio * coordinate2);
float impulse = _mass * (-C);
b1.Sweep.C += b1.InvMass * impulse * _J.LinearA;
b1.Sweep.A += b1.InvI * impulse * _J.AngularA;
b2.Sweep.C += b2.InvMass * impulse * _J.LinearB;
b2.Sweep.A += b2.InvI * impulse * _J.AngularB;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
// TODO_ERIN not implemented
return linearError < Settings.LinearSlop;
}
}
}

282
axios/Dynamics/Joints/Joint.cs Normal file
View File

@@ -0,0 +1,282 @@
/*
* 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 Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
public enum JointType
{
Revolute,
Prismatic,
Distance,
Pulley,
Gear,
Line,
Weld,
Friction,
Slider,
Angle,
Rope,
FixedMouse,
FixedRevolute,
FixedDistance,
FixedLine,
FixedPrismatic,
FixedAngle,
FixedFriction,
}
public enum LimitState
{
Inactive,
AtLower,
AtUpper,
Equal,
}
internal struct Jacobian
{
public float AngularA;
public float AngularB;
public Vector2 LinearA;
public Vector2 LinearB;
public void SetZero()
{
LinearA = Vector2.Zero;
AngularA = 0.0f;
LinearB = Vector2.Zero;
AngularB = 0.0f;
}
public void Set(Vector2 x1, float a1, Vector2 x2, float a2)
{
LinearA = x1;
AngularA = a1;
LinearB = x2;
AngularB = a2;
}
public float Compute(Vector2 x1, float a1, Vector2 x2, float a2)
{
return Vector2.Dot(LinearA, x1) + AngularA * a1 + Vector2.Dot(LinearB, x2) + AngularB * a2;
}
}
/// <summary>
/// A joint edge is used to connect bodies and joints together
/// in a joint graph where each body is a node and each joint
/// is an edge. A joint edge belongs to a doubly linked list
/// maintained in each attached body. Each joint has two joint
/// nodes, one for each attached body.
/// </summary>
public sealed class JointEdge
{
/// <summary>
/// The joint.
/// </summary>
public Joint Joint;
/// <summary>
/// The next joint edge in the body's joint list.
/// </summary>
public JointEdge Next;
/// <summary>
/// Provides quick access to the other body attached.
/// </summary>
public Body Other;
/// <summary>
/// The previous joint edge in the body's joint list.
/// </summary>
public JointEdge Prev;
}
public abstract class Joint
{
/// <summary>
/// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks.
/// The default value is float.MaxValue
/// </summary>
public float Breakpoint = float.MaxValue;
internal JointEdge EdgeA = new JointEdge();
internal JointEdge EdgeB = new JointEdge();
public bool Enabled = true;
protected float InvIA;
protected float InvIB;
protected float InvMassA;
protected float InvMassB;
internal bool IslandFlag;
protected Vector2 LocalCenterA, LocalCenterB;
protected Joint()
{
}
protected Joint(Body body, Body bodyB)
{
Debug.Assert(body != bodyB);
BodyA = body;
BodyB = bodyB;
//Connected bodies should not collide by default
CollideConnected = false;
}
/// <summary>
/// Constructor for fixed joint
/// </summary>
protected Joint(Body body)
{
BodyA = body;
//Connected bodies should not collide by default
CollideConnected = false;
}
/// <summary>
/// Gets or sets the type of the joint.
/// </summary>
/// <value>The type of the joint.</value>
public JointType JointType { get; protected set; }
/// <summary>
/// Get the first body attached to this joint.
/// </summary>
/// <value></value>
public Body BodyA { get; set; }
/// <summary>
/// Get the second body attached to this joint.
/// </summary>
/// <value></value>
public Body BodyB { get; set; }
/// <summary>
/// Get the anchor point on body1 in world coordinates.
/// </summary>
/// <value></value>
public abstract Vector2 WorldAnchorA { get; }
/// <summary>
/// Get the anchor point on body2 in world coordinates.
/// </summary>
/// <value></value>
public abstract Vector2 WorldAnchorB { get; set; }
/// <summary>
/// Set the user data pointer.
/// </summary>
/// <value>The data.</value>
public object UserData { get; set; }
/// <summary>
/// Short-cut function to determine if either body is inactive.
/// </summary>
/// <value><c>true</c> if active; otherwise, <c>false</c>.</value>
public bool Active
{
get { return BodyA.Enabled && BodyB.Enabled; }
}
/// <summary>
/// Set this flag to true if the attached bodies should collide.
/// </summary>
public bool CollideConnected { get; set; }
/// <summary>
/// Fires when the joint is broken.
/// </summary>
public event Action<Joint, float> Broke;
/// <summary>
/// Get the reaction force on body2 at the joint anchor in Newtons.
/// </summary>
/// <param name="inv_dt">The inv_dt.</param>
/// <returns></returns>
public abstract Vector2 GetReactionForce(float inv_dt);
/// <summary>
/// Get the reaction torque on body2 in N*m.
/// </summary>
/// <param name="inv_dt">The inv_dt.</param>
/// <returns></returns>
public abstract float GetReactionTorque(float inv_dt);
protected void WakeBodies()
{
BodyA.Awake = true;
if (BodyB != null)
{
BodyB.Awake = true;
}
}
/// <summary>
/// Return true if the joint is a fixed type.
/// </summary>
public bool IsFixedType()
{
return JointType == JointType.FixedRevolute ||
JointType == JointType.FixedDistance ||
JointType == JointType.FixedPrismatic ||
JointType == JointType.FixedLine ||
JointType == JointType.FixedMouse ||
JointType == JointType.FixedAngle ||
JointType == JointType.FixedFriction;
}
internal abstract void InitVelocityConstraints(ref TimeStep step);
internal void Validate(float invDT)
{
if (!Enabled)
return;
float jointError = GetReactionForce(invDT).Length();
if (Math.Abs(jointError) <= Breakpoint)
return;
Enabled = false;
if (Broke != null)
Broke(this, jointError);
}
internal abstract void SolveVelocityConstraints(ref TimeStep step);
/// <summary>
/// Solves the position constraints.
/// </summary>
/// <returns>returns true if the position errors are within tolerance.</returns>
internal abstract bool SolvePositionConstraints();
}
}

436
axios/Dynamics/Joints/LineJoint.cs Normal file
View File

@@ -0,0 +1,436 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
public class LineJoint : Joint
{
private Vector2 _ax, _ay;
private float _bias;
private bool _enableMotor;
private float _gamma;
private float _impulse;
private Vector2 _localXAxis;
private Vector2 _localYAxisA;
private float _mass;
private float _maxMotorTorque;
private float _motorImpulse;
private float _motorMass;
private float _motorSpeed;
private float _sAx;
private float _sAy;
private float _sBx;
private float _sBy;
private float _springImpulse;
private float _springMass;
// Linear constraint (point-to-line)
// d = pB - pA = xB + rB - xA - rA
// C = dot(ay, d)
// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
// Spring linear constraint
// C = dot(ax, d)
// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
// Motor rotational constraint
// Cdot = wB - wA
// J = [0 0 -1 0 0 1]
internal LineJoint()
{
JointType = JointType.Line;
}
public LineJoint(Body bA, Body bB, Vector2 anchor, Vector2 axis)
: base(bA, bB)
{
JointType = JointType.Line;
LocalAnchorA = bA.GetLocalPoint(anchor);
LocalAnchorB = bB.GetLocalPoint(anchor);
LocalXAxis = bA.GetLocalVector(axis);
}
public Vector2 LocalAnchorA { get; set; }
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public float JointTranslation
{
get
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
Vector2 d = pB - pA;
Vector2 axis = bA.GetWorldVector(LocalXAxis);
float translation = Vector2.Dot(d, axis);
return translation;
}
}
public float JointSpeed
{
get
{
float wA = BodyA.AngularVelocityInternal;
float wB = BodyB.AngularVelocityInternal;
return wB - wA;
}
}
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
BodyA.Awake = true;
BodyB.Awake = true;
_enableMotor = value;
}
}
public float MotorSpeed
{
set
{
BodyA.Awake = true;
BodyB.Awake = true;
_motorSpeed = value;
}
get { return _motorSpeed; }
}
public float MaxMotorTorque
{
set
{
BodyA.Awake = true;
BodyB.Awake = true;
_maxMotorTorque = value;
}
get { return _maxMotorTorque; }
}
public float Frequency { get; set; }
public float DampingRatio { get; set; }
public Vector2 LocalXAxis
{
get { return _localXAxis; }
set
{
_localXAxis = value;
_localYAxisA = MathUtils.Cross(1.0f, _localXAxis);
}
}
public override Vector2 GetReactionForce(float invDt)
{
return invDt * (_impulse * _ay + _springImpulse * _ax);
}
public override float GetReactionTorque(float invDt)
{
return invDt * _motorImpulse;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
LocalCenterA = bA.LocalCenter;
LocalCenterB = bB.LocalCenter;
Transform xfA;
bA.GetTransform(out xfA);
Transform xfB;
bB.GetTransform(out xfB);
// Compute the effective masses.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - LocalCenterA);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB);
Vector2 d = bB.Sweep.C + rB - bA.Sweep.C - rA;
InvMassA = bA.InvMass;
InvIA = bA.InvI;
InvMassB = bB.InvMass;
InvIB = bB.InvI;
// Point to line constraint
{
_ay = MathUtils.Multiply(ref xfA.R, _localYAxisA);
_sAy = MathUtils.Cross(d + rA, _ay);
_sBy = MathUtils.Cross(rB, _ay);
_mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
if (_mass > 0.0f)
{
_mass = 1.0f / _mass;
}
}
// Spring constraint
_springMass = 0.0f;
if (Frequency > 0.0f)
{
_ax = MathUtils.Multiply(ref xfA.R, LocalXAxis);
_sAx = MathUtils.Cross(d + rA, _ax);
_sBx = MathUtils.Cross(rB, _ax);
float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx;
if (invMass > 0.0f)
{
_springMass = 1.0f / invMass;
float C = Vector2.Dot(d, _ax);
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float da = 2.0f * _springMass * DampingRatio * omega;
// Spring stiffness
float k = _springMass * omega * omega;
// magic formulas
_gamma = step.dt * (da + step.dt * k);
if (_gamma > 0.0f)
{
_gamma = 1.0f / _gamma;
}
_bias = C * step.dt * k * _gamma;
_springMass = invMass + _gamma;
if (_springMass > 0.0f)
{
_springMass = 1.0f / _springMass;
}
}
}
else
{
_springImpulse = 0.0f;
_springMass = 0.0f;
}
// Rotational motor
if (_enableMotor)
{
_motorMass = InvIA + InvIB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
else
{
_motorMass = 0.0f;
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_springImpulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse * _ay + _springImpulse * _ax;
float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse;
float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
bA.LinearVelocityInternal -= InvMassA * P;
bA.AngularVelocityInternal -= InvIA * LA;
bB.LinearVelocityInternal += InvMassB * P;
bB.AngularVelocityInternal += InvIB * LB;
}
else
{
_impulse = 0.0f;
_springImpulse = 0.0f;
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 vA = bA.LinearVelocity;
float wA = bA.AngularVelocityInternal;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
// Solve spring constraint
{
float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
_springImpulse += impulse;
Vector2 P = impulse * _ax;
float LA = impulse * _sAx;
float LB = impulse * _sBx;
vA -= InvMassA * P;
wA -= InvIA * LA;
vB += InvMassB * P;
wB += InvIB * LB;
}
// Solve rotational motor constraint
{
float Cdot = wB - wA - _motorSpeed;
float impulse = -_motorMass * Cdot;
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= InvIA * impulse;
wB += InvIB * impulse;
}
// Solve point to line constraint
{
float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
float impulse = _mass * (-Cdot);
_impulse += impulse;
Vector2 P = impulse * _ay;
float LA = impulse * _sAy;
float LB = impulse * _sBy;
vA -= InvMassA * P;
wA -= InvIA * LA;
vB += InvMassB * P;
wB += InvIB * LB;
}
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
internal override bool SolvePositionConstraints()
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 xA = bA.Sweep.C;
float angleA = bA.Sweep.A;
Vector2 xB = bB.Sweep.C;
float angleB = bB.Sweep.A;
Mat22 RA = new Mat22(angleA);
Mat22 RB = new Mat22(angleB);
Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA);
Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB);
Vector2 d = xB + rB - xA - rA;
Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA);
float sAy = MathUtils.Cross(d + rA, ay);
float sBy = MathUtils.Cross(rB, ay);
float C = Vector2.Dot(d, ay);
float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
float impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
Vector2 P = impulse * ay;
float LA = impulse * sAy;
float LB = impulse * sBy;
xA -= InvMassA * P;
angleA -= InvIA * LA;
xB += InvMassB * P;
angleB += InvIB * LB;
// TODO_ERIN remove need for this.
bA.Sweep.C = xA;
bA.Sweep.A = angleA;
bB.Sweep.C = xB;
bB.Sweep.A = angleB;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
return Math.Abs(C) <= Settings.LinearSlop;
}
public float GetMotorTorque(float invDt)
{
return invDt * _motorImpulse;
}
}
}

677
axios/Dynamics/Joints/PrismaticJoint.cs Normal file
View File

@@ -0,0 +1,677 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Linear constraint (point-to-line)
// d = p2 - p1 = x2 + r2 - x1 - r1
// C = dot(perp, d)
// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
//
// Angular constraint
// C = a2 - a1 + a_initial
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
//
// K = J * invM * JT
//
// J = [-a -s1 a s2]
// [0 -1 0 1]
// a = perp
// s1 = cross(d + r1, a) = cross(p2 - x1, a)
// s2 = cross(r2, a) = cross(p2 - x2, a)
// Motor/Limit linear constraint
// C = dot(ax1, d)
// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
// Block Solver
// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
// when the mass has poor distribution (leading to large torques about the joint anchor points).
//
// The Jacobian has 3 rows:
// J = [-uT -s1 uT s2] // linear
// [0 -1 0 1] // angular
// [-vT -a1 vT a2] // limit
//
// u = perp
// v = axis
// s1 = cross(d + r1, u), s2 = cross(r2, u)
// a1 = cross(d + r1, v), a2 = cross(r2, v)
// M * (v2 - v1) = JT * df
// J * v2 = bias
//
// v2 = v1 + invM * JT * df
// J * (v1 + invM * JT * df) = bias
// K * df = bias - J * v1 = -Cdot
// K = J * invM * JT
// Cdot = J * v1 - bias
//
// Now solve for f2.
// df = f2 - f1
// K * (f2 - f1) = -Cdot
// f2 = invK * (-Cdot) + f1
//
// Clamp accumulated limit impulse.
// lower: f2(3) = max(f2(3), 0)
// upper: f2(3) = min(f2(3), 0)
//
// Solve for correct f2(1:2)
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
//
// Now compute impulse to be applied:
// df = f2 - f1
/// <summary>
/// A prismatic joint. This joint provides one degree of freedom: translation
/// along an axis fixed in body1. Relative rotation is prevented. You can
/// use a joint limit to restrict the range of motion and a joint motor to
/// drive the motion or to model joint friction.
/// </summary>
public class PrismaticJoint : Joint
{
public Vector2 LocalAnchorA;
public Vector2 LocalAnchorB;
private Mat33 _K;
private float _a1, _a2;
private Vector2 _axis;
private bool _enableLimit;
private bool _enableMotor;
private Vector3 _impulse;
private LimitState _limitState;
private Vector2 _localXAxis1;
private Vector2 _localYAxis1;
private float _lowerTranslation;
private float _maxMotorForce;
private float _motorImpulse;
private float _motorMass; // effective mass for motor/limit translational constraint.
private float _motorSpeed;
private Vector2 _perp;
private float _refAngle;
private float _s1, _s2;
private float _upperTranslation;
internal PrismaticJoint()
{
JointType = JointType.Prismatic;
}
/// <summary>
/// This requires defining a line of
/// motion using an axis and an anchor point. The definition uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
/// <param name="axis">The axis.</param>
public PrismaticJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis)
: base(bodyA, bodyB)
{
JointType = JointType.Prismatic;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
_localXAxis1 = BodyA.GetLocalVector(axis);
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_refAngle = BodyB.Rotation - BodyA.Rotation;
_limitState = LimitState.Inactive;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// Get the current joint translation, usually in meters.
/// </summary>
/// <value></value>
public float JointTranslation
{
get
{
Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA);
Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1);
return Vector2.Dot(d, axis);
}
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
/// <value></value>
public float JointSpeed
{
get
{
Transform xf1, xf2;
BodyA.GetTransform(out xf1);
BodyB.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - BodyA.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter);
Vector2 p1 = BodyA.Sweep.C + r1;
Vector2 p2 = BodyB.Sweep.C + r2;
Vector2 d = p2 - p1;
Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1);
Vector2 v1 = BodyA.LinearVelocityInternal;
Vector2 v2 = BodyB.LinearVelocityInternal;
float w1 = BodyA.AngularVelocityInternal;
float w2 = BodyB.AngularVelocityInternal;
float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) +
Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
return speed;
}
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public bool LimitEnabled
{
get { return _enableLimit; }
set
{
Debug.Assert(BodyA.FixedRotation == false || BodyB.FixedRotation == false,
"Warning: limits does currently not work with fixed rotation");
WakeBodies();
_enableLimit = value;
}
}
/// <summary>
/// Get the lower joint limit, usually in meters.
/// </summary>
/// <value></value>
public float LowerLimit
{
get { return _lowerTranslation; }
set
{
WakeBodies();
_lowerTranslation = value;
}
}
/// <summary>
/// Get the upper joint limit, usually in meters.
/// </summary>
/// <value></value>
public float UpperLimit
{
get { return _upperTranslation; }
set
{
WakeBodies();
_upperTranslation = value;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Set the motor speed, usually in meters per second.
/// </summary>
/// <value>The speed.</value>
public float MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get { return _motorSpeed; }
}
/// <summary>
/// Set the maximum motor force, usually in N.
/// </summary>
/// <value>The force.</value>
public float MaxMotorForce
{
get { return _maxMotorForce; }
set
{
WakeBodies();
_maxMotorForce = value;
}
}
/// <summary>
/// Get the current motor force, usually in N.
/// </summary>
/// <value></value>
public float MotorForce
{
get { return _motorImpulse; }
set { _motorImpulse = value; }
}
public Vector2 LocalXAxis1
{
get { return _localXAxis1; }
set
{
_localXAxis1 = BodyA.GetLocalVector(value);
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
}
}
public float ReferenceAngle
{
get { return _refAngle; }
set { _refAngle = value; }
}
public override Vector2 GetReactionForce(float inv_dt)
{
return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis);
}
public override float GetReactionTorque(float inv_dt)
{
return inv_dt * _impulse.Y;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
LocalCenterA = b1.LocalCenter;
LocalCenterB = b2.LocalCenter;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
InvMassA = b1.InvMass;
InvIA = b1.InvI;
InvMassB = b2.InvMass;
InvIB = b2.InvI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if (_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
b1.LinearVelocityInternal -= InvMassA * P;
b1.AngularVelocityInternal -= InvIA * L1;
b2.LinearVelocityInternal += InvMassB * P;
b2.AngularVelocityInternal += InvIB * L2;
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float L1 = df.X * _s1 + df.Y + df.Z * _a1;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float L1 = df.X * _s1 + df.Y;
float L2 = df.X * _s2 + df.Y;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
internal override bool SolvePositionConstraints()
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 c1 = b1.Sweep.C;
float a1 = b1.Sweep.A;
Vector2 c2 = b2.Sweep.C;
float a2 = b2.Sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
float angularError = Math.Abs(C1.Y);
if (active)
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C); // negated above
}
else
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c1 -= InvMassA * P;
a1 -= InvIA * L1;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b1.Sweep.C = c1;
b1.Sweep.A = a1;
b2.Sweep.C = c2;
b2.Sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}

507
axios/Dynamics/Joints/PulleyJoint.cs Normal file
View File

@@ -0,0 +1,507 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// The pulley joint is connected to two bodies and two fixed ground points.
/// The pulley supports a ratio such that:
/// length1 + ratio * length2 <!--<-->= ant
/// Yes, the force transmitted is scaled by the ratio.
/// The pulley also enforces a maximum length limit on both sides. This is
/// useful to prevent one side of the pulley hitting the top.
/// </summary>
public class PulleyJoint : Joint
{
/// <summary>
/// Get the first ground anchor.
/// </summary>
/// <value></value>
public Vector2 GroundAnchorA;
/// <summary>
/// Get the second ground anchor.
/// </summary>
/// <value></value>
public Vector2 GroundAnchorB;
public Vector2 LocalAnchorA;
public Vector2 LocalAnchorB;
public float MinPulleyLength = 2.0f;
private float _ant;
private float _impulse;
private float _lengthA;
private float _lengthB;
private float _limitImpulse1;
private float _limitImpulse2;
private float _limitMass1;
private float _limitMass2;
private LimitState _limitState1;
private LimitState _limitState2;
private float _maxLengthA;
private float _maxLengthB;
// Effective masses
private float _pulleyMass;
private LimitState _state;
private Vector2 _u1;
private Vector2 _u2;
internal PulleyJoint()
{
JointType = JointType.Pulley;
}
/// <summary>
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
/// This requires two ground anchors,
/// two dynamic body anchor points, max lengths for each side,
/// and a pulley ratio.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="groundAnchorA">The ground anchor for the first body.</param>
/// <param name="groundAnchorB">The ground anchor for the second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
/// <param name="ratio">The ratio.</param>
public PulleyJoint(Body bodyA, Body bodyB,
Vector2 groundAnchorA, Vector2 groundAnchorB,
Vector2 localAnchorA, Vector2 localAnchorB,
float ratio)
: base(bodyA, bodyB)
{
JointType = JointType.Pulley;
GroundAnchorA = groundAnchorA;
GroundAnchorB = groundAnchorB;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
Vector2 d1 = BodyA.GetWorldPoint(localAnchorA) - groundAnchorA;
_lengthA = d1.Length();
Vector2 d2 = BodyB.GetWorldPoint(localAnchorB) - groundAnchorB;
_lengthB = d2.Length();
Debug.Assert(ratio != 0.0f);
Debug.Assert(ratio > Settings.Epsilon);
Ratio = ratio;
float C = _lengthA + Ratio * _lengthB;
MaxLengthA = C - Ratio * MinPulleyLength;
MaxLengthB = (C - MinPulleyLength) / Ratio;
_ant = _lengthA + Ratio * _lengthB;
MaxLengthA = Math.Min(MaxLengthA, _ant - Ratio * MinPulleyLength);
MaxLengthB = Math.Min(MaxLengthB, (_ant - MinPulleyLength) / Ratio);
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// Get the current length of the segment attached to body1.
/// </summary>
/// <value></value>
public float LengthA
{
get
{
Vector2 d = BodyA.GetWorldPoint(LocalAnchorA) - GroundAnchorA;
return d.Length();
}
set { _lengthA = value; }
}
/// <summary>
/// Get the current length of the segment attached to body2.
/// </summary>
/// <value></value>
public float LengthB
{
get
{
Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - GroundAnchorB;
return d.Length();
}
set { _lengthB = value; }
}
/// <summary>
/// Get the pulley ratio.
/// </summary>
/// <value></value>
public float Ratio { get; set; }
public float MaxLengthA
{
get { return _maxLengthA; }
set { _maxLengthA = value; }
}
public float MaxLengthB
{
get { return _maxLengthB; }
set { _maxLengthB = value; }
}
public override Vector2 GetReactionForce(float inv_dt)
{
Vector2 P = _impulse * _u2;
return inv_dt * P;
}
public override float GetReactionTorque(float inv_dt)
{
return 0.0f;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p1 = b1.Sweep.C + r1;
Vector2 p2 = b2.Sweep.C + r2;
Vector2 s1 = GroundAnchorA;
Vector2 s2 = GroundAnchorB;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - Ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < MaxLengthA)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < MaxLengthB)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1;
_limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.Epsilon);
Debug.Assert(_limitMass2 > Settings.Epsilon);
Debug.Assert(_pulleyMass > Settings.Epsilon);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
if (_state == LimitState.AtUpper)
{
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = -Vector2.Dot(_u1, v1) - Ratio * Vector2.Dot(_u2, v2);
float impulse = _pulleyMass * (-Cdot);
float oldImpulse = _impulse;
_impulse = Math.Max(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -Ratio * impulse * _u2;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpper)
{
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
float Cdot = -Vector2.Dot(_u1, v1);
float impulse = -_limitMass1 * Cdot;
float oldImpulse = _limitImpulse1;
_limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
impulse = _limitImpulse1 - oldImpulse;
Vector2 P1 = -impulse * _u1;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpper)
{
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = -Vector2.Dot(_u2, v2);
float impulse = -_limitMass2 * Cdot;
float oldImpulse = _limitImpulse2;
_limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
impulse = _limitImpulse2 - oldImpulse;
Vector2 P2 = -impulse * _u2;
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
}
internal override bool SolvePositionConstraints()
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 s1 = GroundAnchorA;
Vector2 s2 = GroundAnchorB;
float linearError = 0.0f;
if (_state == LimitState.AtUpper)
{
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p1 = b1.Sweep.C + r1;
Vector2 p2 = b2.Sweep.C + r2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - Ratio * length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -Ratio * impulse * _u2;
b1.Sweep.C += b1.InvMass * P1;
b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
b2.Sweep.C += b2.InvMass * P2;
b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpper)
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 p1 = b1.Sweep.C + r1;
_u1 = p1 - s1;
float length1 = _u1.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
float C = MaxLengthA - length1;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vector2 P1 = -impulse * _u1;
b1.Sweep.C += b1.InvMass * P1;
b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpper)
{
Transform xf2;
b2.GetTransform(out xf2);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p2 = b2.Sweep.C + r2;
_u2 = p2 - s2;
float length2 = _u2.Length();
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = MaxLengthB - length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vector2 P2 = -impulse * _u2;
b2.Sweep.C += b2.InvMass * P2;
b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
b2.SynchronizeTransform();
}
return linearError < Settings.LinearSlop;
}
}
}

595
axios/Dynamics/Joints/RevoluteJoint.cs Normal file
View File

@@ -0,0 +1,595 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// A revolute joint rains to bodies to share a common point while they
/// are free to rotate about the point. The relative rotation about the shared
/// point is the joint angle. You can limit the relative rotation with
/// a joint limit that specifies a lower and upper angle. You can use a motor
/// to drive the relative rotation about the shared point. A maximum motor torque
/// is provided so that infinite forces are not generated.
/// </summary>
public class RevoluteJoint : Joint
{
public Vector2 LocalAnchorA;
public Vector2 LocalAnchorB;
private bool _enableLimit;
private bool _enableMotor;
private Vector3 _impulse;
private LimitState _limitState;
private float _lowerAngle;
private Mat33 _mass; // effective mass for point-to-point constraint.
private float _maxMotorTorque;
private float _motorImpulse;
private float _motorMass; // effective mass for motor/limit angular constraint.
private float _motorSpeed;
private float _referenceAngle;
private float _tmpFloat1;
private Vector2 _tmpVector1, _tmpVector2;
private float _upperAngle;
internal RevoluteJoint()
{
JointType = JointType.Revolute;
}
/// <summary>
/// Initialize the bodies and local anchor.
/// This requires defining an
/// anchor point where the bodies are joined. The definition
/// uses local anchor points so that the initial configuration
/// can violate the constraint slightly. You also need to
/// specify the initial relative angle for joint limits. This
/// helps when saving and loading a game.
/// The local anchor points are measured from the body's origin
/// rather than the center of mass because:
/// 1. you might not know where the center of mass will be.
/// 2. if you add/remove shapes from a body and recompute the mass,
/// the joints will be broken.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second anchor.</param>
public RevoluteJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Revolute;
// Changed to local coordinates.
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
_impulse = Vector3.Zero;
_limitState = LimitState.Inactive;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public float ReferenceAngle
{
get { return _referenceAngle; }
set
{
WakeBodies();
_referenceAngle = value;
}
}
/// <summary>
/// Get the current joint angle in radians.
/// </summary>
/// <value></value>
public float JointAngle
{
get { return BodyB.Sweep.A - BodyA.Sweep.A - ReferenceAngle; }
}
/// <summary>
/// Get the current joint angle speed in radians per second.
/// </summary>
/// <value></value>
public float JointSpeed
{
get { return BodyB.AngularVelocityInternal - BodyA.AngularVelocityInternal; }
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public bool LimitEnabled
{
get { return _enableLimit; }
set
{
WakeBodies();
_enableLimit = value;
}
}
/// <summary>
/// Get the lower joint limit in radians.
/// </summary>
/// <value></value>
public float LowerLimit
{
get { return _lowerAngle; }
set
{
WakeBodies();
_lowerAngle = value;
}
}
/// <summary>
/// Get the upper joint limit in radians.
/// </summary>
/// <value></value>
public float UpperLimit
{
get { return _upperAngle; }
set
{
WakeBodies();
_upperAngle = value;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Set the motor speed in radians per second.
/// </summary>
/// <value>The speed.</value>
public float MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get { return _motorSpeed; }
}
/// <summary>
/// Set the maximum motor torque, usually in N-m.
/// </summary>
/// <value>The torque.</value>
public float MaxMotorTorque
{
set
{
WakeBodies();
_maxMotorTorque = value;
}
get { return _maxMotorTorque; }
}
/// <summary>
/// Get the current motor torque, usually in N-m.
/// </summary>
/// <value></value>
public float MotorTorque
{
get { return _motorImpulse; }
set
{
WakeBodies();
_motorImpulse = value;
}
}
public override Vector2 GetReactionForce(float inv_dt)
{
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
return inv_dt * P;
}
public override float GetReactionTorque(float inv_dt)
{
return inv_dt * _impulse.Z;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Debug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f);
}
// Compute the effective mass matrix.
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1.InvMass, m2 = b2.InvMass;
float i1 = b1.InvI, i2 = b2.InvI;
_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.Col3.Y = r1.X * i1 + r2.X * i2;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = i1 + i2;
_motorMass = i1 + i2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
b1.LinearVelocityInternal -= m1 * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
b1.AngularVelocityInternal -= i1 * ( /* r1 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
b2.LinearVelocityInternal += m2 * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
b2.AngularVelocityInternal += i2 * ( /* r2 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
float m1 = b1.InvMass, m2 = b2.InvMass;
float i1 = b1.InvI, i2 = b2.InvI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
// Solve point-to-point constraint
MathUtils.Cross(w2, ref r2, out _tmpVector2);
MathUtils.Cross(w1, ref r1, out _tmpVector1);
Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
v1 -= m1 * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
w1 -= i1 * ( /* r1 x P */_tmpFloat1 + impulse.Z);
v2 += m2 * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
w2 += i2 * ( /* r2 x P */_tmpFloat1 + impulse.Z);
}
else
{
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
_tmpVector1 = LocalAnchorA - b1.LocalCenter;
_tmpVector2 = LocalAnchorB - b2.LocalCenter;
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2);
// Solve point-to-point constraint
MathUtils.Cross(w2, ref r2, out _tmpVector2);
MathUtils.Cross(w1, ref r1, out _tmpVector1);
Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
w1 -= i1 * /* r1 x impulse */ _tmpFloat1;
v2 += m2 * impulse;
MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
w2 += i2 * /* r2 x impulse */ _tmpFloat1;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
Body b2 = BodyB;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b2.Sweep.A += b2.InvI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
positionError = C.Length();
float invMass1 = b1.InvMass, invMass2 = b2.InvMass;
float invI1 = b1.InvI, invI2 = b2.InvI;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C;
u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const float k_beta = 0.5f;
b1.Sweep.C -= k_beta * invMass1 * impulse2;
b2.Sweep.C += k_beta * invMass2 * impulse2;
C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
Mat22 Ka;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22 K;
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1.Sweep.C -= b1.InvMass * impulse;
MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1;
b2.Sweep.C += b2.InvMass * impulse;
MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}

239
axios/Dynamics/Joints/RopeJoint.cs Normal file
View File

@@ -0,0 +1,239 @@
/*
* Copyright (c) 2006-2010 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Limit:
// C = norm(pB - pA) - L
// u = (pB - pA) / norm(pB - pA)
// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))
// J = [-u -cross(rA, u) u cross(rB, u)]
// K = J * invM * JT
// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2
/// <summary>
/// A rope joint enforces a maximum distance between two points
/// on two bodies. It has no other effect.
/// Warning: if you attempt to change the maximum length during
/// the simulation you will get some non-physical behavior.
/// A model that would allow you to dynamically modify the length
/// would have some sponginess, so I chose not to implement it
/// that way. See b2DistanceJoint if you want to dynamically
/// control length.
/// </summary>
public class RopeJoint : Joint
{
public Vector2 LocalAnchorA;
public Vector2 LocalAnchorB;
private float _impulse;
private float _length;
private float _mass;
private Vector2 _rA, _rB;
private LimitState _state;
private Vector2 _u;
internal RopeJoint()
{
JointType = JointType.Rope;
}
public RopeJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Rope;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
Vector2 d = WorldAnchorB - WorldAnchorA;
MaxLength = d.Length();
_mass = 0.0f;
_impulse = 0.0f;
_state = LimitState.Inactive;
_length = 0.0f;
}
/// Get the maximum length of the rope.
public float MaxLength { get; set; }
public LimitState State
{
get { return _state; }
}
public override sealed Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override sealed Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 GetReactionForce(float invDt)
{
return (invDt * _impulse) * _u;
}
public override float GetReactionTorque(float invDt)
{
return 0;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xf1;
bA.GetTransform(out xf1);
Transform xf2;
bB.GetTransform(out xf2);
_rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
_rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
// Rope axis
_u = bB.Sweep.C + _rB - bA.Sweep.C - _rA;
_length = _u.Length();
float C = _length - MaxLength;
if (C > 0.0f)
{
_state = LimitState.AtUpper;
}
else
{
_state = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
bA.LinearVelocity -= bA.InvMass * P;
bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
bB.LinearVelocity += bB.InvMass * P;
bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
// Cdot = dot(u, v + cross(w, r))
Vector2 vA = bA.LinearVelocity + MathUtils.Cross(bA.AngularVelocity, _rA);
Vector2 vB = bB.LinearVelocity + MathUtils.Cross(bB.AngularVelocity, _rB);
float C = _length - MaxLength;
float Cdot = Vector2.Dot(_u, vB - vA);
// Predictive constraint.
if (C < 0.0f)
{
Cdot += step.inv_dt * C;
}
float impulse = -_mass * Cdot;
float oldImpulse = _impulse;
_impulse = Math.Min(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P = impulse * _u;
bA.LinearVelocity -= bA.InvMass * P;
bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
bB.LinearVelocity += bB.InvMass * P;
bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
}
internal override bool SolvePositionConstraints()
{
Body bA = BodyA;
Body bB = BodyB;
Transform xf1;
bA.GetTransform(out xf1);
Transform xf2;
bB.GetTransform(out xf2);
Vector2 rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
Vector2 u = bB.Sweep.C + rB - bA.Sweep.C - rA;
float length = u.Length();
u.Normalize();
float C = length - MaxLength;
C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
Vector2 P = impulse * u;
bA.Sweep.C -= bA.InvMass * P;
bA.Sweep.A -= bA.InvI * MathUtils.Cross(rA, P);
bB.Sweep.C += bB.InvMass * P;
bB.Sweep.A += bB.InvI * MathUtils.Cross(rB, P);
bA.SynchronizeTransform();
bB.SynchronizeTransform();
return length - MaxLength < Settings.LinearSlop;
}
}
}

298
axios/Dynamics/Joints/SliderJoint.cs Normal file
View File

@@ -0,0 +1,298 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// A distance joint contrains two points on two bodies
/// to remain at a fixed distance from each other. You can view
/// this as a massless, rigid rod.
/// </summary>
public class SliderJoint : Joint
{
// 1-D constrained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
public Vector2 LocalAnchorA;
public Vector2 LocalAnchorB;
private float _bias;
private float _gamma;
private float _impulse;
private float _mass;
private Vector2 _u;
internal SliderJoint()
{
JointType = JointType.Slider;
}
/// <summary>
/// Initializes a new instance of the <see cref="SliderJoint"/> class.
/// Warning: Do not use a zero or short length.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
/// <param name="minLength">The minimum length between anchorpoints</param>
/// <param name="maxlength">The maximum length between anchorpoints.</param>
public SliderJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, float minLength,
float maxlength)
: base(bodyA, bodyB)
{
JointType = JointType.Slider;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
MaxLength = maxlength;
MinLength = minLength;
}
/// <summary>
/// The maximum length between the anchor points.
/// </summary>
/// <value>The length.</value>
public float MaxLength { get; set; }
/// <summary>
/// The minimal length between the anchor points.
/// </summary>
/// <value>The length.</value>
public float MinLength { get; set; }
/// <summary>
/// The mass-spring-damper frequency in Hertz.
/// </summary>
/// <value>The frequency.</value>
public float Frequency { get; set; }
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
/// <value>The damping ratio.</value>
public float DampingRatio { get; set; }
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 GetReactionForce(float inv_dt)
{
Vector2 F = (inv_dt * _impulse) * _u;
return F;
}
public override float GetReactionTorque(float inv_dt)
{
return 0.0f;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
_u = b2.Sweep.C + r2 - b1.Sweep.C - r1;
// Handle singularity.
float length = _u.Length();
if (length < MaxLength && length > MinLength)
{
return;
}
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u;
Debug.Assert(invMass > Settings.Epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Frequency > 0.0f)
{
float C = length - MaxLength;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _mass * DampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
b2.LinearVelocityInternal += b2.InvMass * P;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
float length = d.Length();
if (length < MaxLength && length > MinLength)
{
return;
}
// Cdot = dot(u, v + cross(w, r))
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
b2.LinearVelocityInternal += b2.InvMass * P;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
}
internal override bool SolvePositionConstraints()
{
if (Frequency > 0.0f)
{
// There is no position correction for soft distance constraints.
return true;
}
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
float length = d.Length();
if (length < MaxLength && length > MinLength)
{
return true;
}
if (length == 0.0f)
return true;
d /= length;
float C = length - MaxLength;
C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
_u = d;
Vector2 P = impulse * _u;
b1.Sweep.C -= b1.InvMass * P;
b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P);
b2.Sweep.C += b2.InvMass * P;
b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return Math.Abs(C) < Settings.LinearSlop;
}
}
}

263
axios/Dynamics/Joints/WeldJoint.cs Normal file
View File

@@ -0,0 +1,263 @@
/*
* 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.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Point-to-point constraint
// C = p2 - p1
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// C = angle2 - angle1 - referenceAngle
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
/// <summary>
/// A weld joint essentially glues two bodies together. A weld joint may
/// distort somewhat because the island constraint solver is approximate.
/// </summary>
public class WeldJoint : Joint
{
public Vector2 LocalAnchorA;
public Vector2 LocalAnchorB;
private Vector3 _impulse;
private Mat33 _mass;
internal WeldJoint()
{
JointType = JointType.Weld;
}
/// <summary>
/// You need to specify a local anchor point
/// where they are attached and the relative body angle. The position
/// of the anchor point is important for computing the reaction torque.
/// You can change the anchor points relative to bodyA or bodyB by changing LocalAnchorA
/// and/or LocalAnchorB.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
public WeldJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Weld;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// The body2 angle minus body1 angle in the reference state (radians).
/// </summary>
public float ReferenceAngle { get; private set; }
public override Vector2 GetReactionForce(float inv_dt)
{
return inv_dt * new Vector2(_impulse.X, _impulse.Y);
}
public override float GetReactionTorque(float inv_dt)
{
return inv_dt * _impulse.Z;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
_mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
_mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
_mass.Col3.X = -rA.Y * iA - rB.Y * iB;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
_mass.Col3.Y = rA.X * iA + rB.X * iB;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = iA + iB;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _impulse.Z);
bB.LinearVelocityInternal += mB * P;
bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 vA = bA.LinearVelocityInternal;
float wA = bA.AngularVelocityInternal;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// Solve point-to-point constraint
Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
_impulse += impulse;
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
internal override bool SolvePositionConstraints()
{
Body bA = BodyA;
Body bB = BodyB;
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Transform xfA;
Transform xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
Vector2 C1 = bB.Sweep.C + rB - bA.Sweep.C - rA;
float C2 = bB.Sweep.A - bA.Sweep.A - ReferenceAngle;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
float positionError = C1.Length();
float angularError = Math.Abs(C2);
if (positionError > k_allowedStretch)
{
iA *= 1.0f;
iB *= 1.0f;
}
_mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
_mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
_mass.Col3.X = -rA.Y * iA - rB.Y * iB;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
_mass.Col3.Y = rA.X * iA + rB.X * iB;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = iA + iB;
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse = _mass.Solve33(-C);
Vector2 P = new Vector2(impulse.X, impulse.Y);
bA.Sweep.C -= mA * P;
bA.Sweep.A -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
bB.Sweep.C += mB * P;
bB.Sweep.A += iB * (MathUtils.Cross(rB, P) + impulse.Z);
bA.SynchronizeTransform();
bB.SynchronizeTransform();
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}

45
axios/Dynamics/TimeStep.cs Normal file
View File

@@ -0,0 +1,45 @@
/*
* 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.
*/
namespace FarseerPhysics.Dynamics
{
/// <summary>
/// This is an internal structure.
/// </summary>
public struct TimeStep
{
/// <summary>
/// Time step (Delta time)
/// </summary>
public float dt;
/// <summary>
/// dt * inv_dt0
/// </summary>
public float dtRatio;
/// <summary>
/// Inverse time step (0 if dt == 0).
/// </summary>
public float inv_dt;
}
}

1456
axios/Dynamics/World.cs Normal file
View File

File diff suppressed because it is too large Load Diff

74
axios/Dynamics/WorldCallbacks.cs Normal file
View File

@@ -0,0 +1,74 @@
/*
* 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 FarseerPhysics.Collision;
using FarseerPhysics.Controllers;
using FarseerPhysics.Dynamics.Contacts;
using FarseerPhysics.Dynamics.Joints;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics
{
/// <summary>
/// Called for each fixture found in the query. You control how the ray cast
/// proceeds by returning a float:
/// <returns>-1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue</returns>
/// </summary>
public delegate float RayCastCallback(Fixture fixture, Vector2 point, Vector2 normal, float fraction);
/// <summary>
/// This delegate is called when a contact is deleted
/// </summary>
public delegate void EndContactDelegate(Contact contact);
/// <summary>
/// This delegate is called when a contact is created
/// </summary>
public delegate bool BeginContactDelegate(Contact contact);
public delegate void PreSolveDelegate(Contact contact, ref Manifold oldManifold);
public delegate void PostSolveDelegate(Contact contact, ContactConstraint impulse);
public delegate void FixtureDelegate(Fixture fixture);
public delegate void JointDelegate(Joint joint);
public delegate void BodyDelegate(Body body);
public delegate void ControllerDelegate(Controller controller);
public delegate bool CollisionFilterDelegate(Fixture fixtureA, Fixture fixtureB);
public delegate void BroadphaseDelegate(ref FixtureProxy proxyA, ref FixtureProxy proxyB);
public delegate bool BeforeCollisionEventHandler(Fixture fixtureA, Fixture fixtureB);
public delegate bool OnCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact);
public delegate void AfterCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact);
public delegate void OnSeparationEventHandler(Fixture fixtureA, Fixture fixtureB);
}