Server job assigning logic, submarine movement syncing, submarine collision improvements, spawnpoints in levels
This commit is contained in:
Regalis
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,148 @@
|
||||
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
|
||||
{
|
||||
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)
|
||||
{
|
||||
_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);
|
||||
Parts.Add(fixture);
|
||||
}
|
||||
}
|
||||
|
||||
public BreakableBody(IEnumerable<Shape> shapes, World world)
|
||||
{
|
||||
_world = world;
|
||||
_world.ContactManager.PostSolve += PostSolve;
|
||||
MainBody = new Body(_world);
|
||||
MainBody.BodyType = BodyType.Dynamic;
|
||||
|
||||
foreach (Shape part in shapes)
|
||||
{
|
||||
Fixture fixture = MainBody.CreateFixture(part);
|
||||
Parts.Add(fixture);
|
||||
}
|
||||
}
|
||||
|
||||
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 void PostSolve(Contact contact, ContactVelocityConstraint 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 oldFixture = Parts[i];
|
||||
|
||||
Shape shape = oldFixture.Shape.Clone();
|
||||
object userData = oldFixture.UserData;
|
||||
|
||||
MainBody.DestroyFixture(oldFixture);
|
||||
|
||||
Body body = BodyFactory.CreateBody(_world);
|
||||
body.BodyType = BodyType.Dynamic;
|
||||
body.Position = MainBody.Position;
|
||||
body.Rotation = MainBody.Rotation;
|
||||
body.UserData = MainBody.UserData;
|
||||
|
||||
Fixture newFixture = body.CreateFixture(shape);
|
||||
newFixture.UserData = userData;
|
||||
Parts[i] = newFixture;
|
||||
|
||||
body.AngularVelocity = _angularVelocitiesCache[i];
|
||||
body.LinearVelocity = _velocitiesCache[i];
|
||||
}
|
||||
|
||||
_world.RemoveBody(MainBody);
|
||||
_world.RemoveBreakableBody(this);
|
||||
}
|
||||
|
||||
public void Break()
|
||||
{
|
||||
_break = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,445 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
//#define USE_ACTIVE_CONTACT_SET
|
||||
|
||||
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);
|
||||
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
/// <summary>
|
||||
/// The set of active contacts.
|
||||
/// </summary>
|
||||
public HashSet<Contact> ActiveContacts = new HashSet<Contact>();
|
||||
|
||||
/// <summary>
|
||||
/// A temporary copy of active contacts that is used during updates so
|
||||
/// the hash set can have members added/removed during the update.
|
||||
/// This list is cleared after every update.
|
||||
/// </summary>
|
||||
List<Contact> ActiveList = new List<Contact>();
|
||||
#endif
|
||||
|
||||
/// <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;
|
||||
|
||||
//FPE feature: BeforeCollision delegate
|
||||
if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false)
|
||||
return;
|
||||
|
||||
if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false)
|
||||
return;
|
||||
|
||||
// Call the factory.
|
||||
Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
|
||||
|
||||
if (c == null)
|
||||
return;
|
||||
|
||||
// Contact creation may swap fixtures.
|
||||
fixtureA = c.FixtureA;
|
||||
fixtureB = c.FixtureB;
|
||||
bodyA = fixtureA.Body;
|
||||
bodyB = fixtureB.Body;
|
||||
|
||||
// Insert into the world.
|
||||
ContactList.Add(c);
|
||||
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
ActiveContacts.Add(c);
|
||||
#endif
|
||||
// Connect to island graph.
|
||||
|
||||
// Connect to body A
|
||||
c._nodeA.Contact = c;
|
||||
c._nodeA.Other = bodyB;
|
||||
|
||||
c._nodeA.Prev = null;
|
||||
c._nodeA.Next = bodyA.ContactList;
|
||||
if (bodyA.ContactList != null)
|
||||
{
|
||||
bodyA.ContactList.Prev = c._nodeA;
|
||||
}
|
||||
bodyA.ContactList = c._nodeA;
|
||||
|
||||
// Connect to body B
|
||||
c._nodeB.Contact = c;
|
||||
c._nodeB.Other = bodyA;
|
||||
|
||||
c._nodeB.Prev = null;
|
||||
c._nodeB.Next = bodyB.ContactList;
|
||||
if (bodyB.ContactList != null)
|
||||
{
|
||||
bodyB.ContactList.Prev = c._nodeB;
|
||||
}
|
||||
bodyB.ContactList = c._nodeB;
|
||||
|
||||
// Wake up the bodies
|
||||
if (fixtureA.IsSensor == false && fixtureB.IsSensor == false)
|
||||
{
|
||||
bodyA.Awake = true;
|
||||
bodyB.Awake = true;
|
||||
}
|
||||
}
|
||||
|
||||
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 (contact.IsTouching)
|
||||
{
|
||||
//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 (EndContact != null)
|
||||
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;
|
||||
}
|
||||
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
if (ActiveContacts.Contains(contact))
|
||||
{
|
||||
ActiveContacts.Remove(contact);
|
||||
}
|
||||
#endif
|
||||
contact.Destroy();
|
||||
}
|
||||
|
||||
internal void Collide()
|
||||
{
|
||||
// Update awake contacts.
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
ActiveList.AddRange(ActiveContacts);
|
||||
|
||||
foreach (var c in ActiveList)
|
||||
{
|
||||
#else
|
||||
for (int i = 0; i < ContactList.Count; i++)
|
||||
{
|
||||
Contact c = ContactList[i];
|
||||
#endif
|
||||
Fixture fixtureA = c.FixtureA;
|
||||
Fixture fixtureB = c.FixtureB;
|
||||
int indexA = c.ChildIndexA;
|
||||
int indexB = c.ChildIndexB;
|
||||
Body bodyA = fixtureA.Body;
|
||||
Body bodyB = fixtureB.Body;
|
||||
|
||||
//Do no try to collide disabled bodies
|
||||
if (!bodyA.Enabled || !bodyB.Enabled)
|
||||
continue;
|
||||
|
||||
// Is this contact flagged for filtering?
|
||||
if (c.FilterFlag)
|
||||
{
|
||||
// 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.FilterFlag = false;
|
||||
}
|
||||
|
||||
bool activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static;
|
||||
bool activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static;
|
||||
|
||||
// At least one body must be awake and it must be dynamic or kinematic.
|
||||
if (activeA == false && activeB == false)
|
||||
{
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
ActiveContacts.Remove(c);
|
||||
#endif
|
||||
continue;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
ActiveList.Clear();
|
||||
#endif
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
internal void UpdateContacts(ContactEdge contactEdge, bool value)
|
||||
{
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
if(value)
|
||||
{
|
||||
while(contactEdge != null)
|
||||
{
|
||||
var c = contactEdge.Contact;
|
||||
if (!ActiveContacts.Contains(c))
|
||||
{
|
||||
ActiveContacts.Add(c);
|
||||
}
|
||||
contactEdge = contactEdge.Next;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
while (contactEdge != null)
|
||||
{
|
||||
var c = contactEdge.Contact;
|
||||
if (!contactEdge.Other.Awake)
|
||||
{
|
||||
if (ActiveContacts.Contains(c))
|
||||
{
|
||||
ActiveContacts.Remove(c);
|
||||
}
|
||||
}
|
||||
contactEdge = contactEdge.Next;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
internal void RemoveActiveContact(Contact contact)
|
||||
{
|
||||
if (ActiveContacts.Contains(contact))
|
||||
ActiveContacts.Remove(contact);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,480 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
//#define USE_ACTIVE_CONTACT_SET
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/// <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 ContactType _type;
|
||||
|
||||
private static EdgeShape _edge = new EdgeShape();
|
||||
|
||||
private static ContactType[,] _registers = new[,]
|
||||
{
|
||||
{
|
||||
ContactType.Circle,
|
||||
ContactType.EdgeAndCircle,
|
||||
ContactType.PolygonAndCircle,
|
||||
ContactType.ChainAndCircle,
|
||||
},
|
||||
{
|
||||
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.ChainAndPolygon,
|
||||
},
|
||||
{
|
||||
ContactType.ChainAndCircle,
|
||||
ContactType.NotSupported,
|
||||
// 3,1 is invalid (no ContactType.EdgeAndLoop)
|
||||
ContactType.ChainAndPolygon,
|
||||
ContactType.NotSupported,
|
||||
// 3,3 is invalid (no ContactType.Loop)
|
||||
},
|
||||
};
|
||||
// Nodes for connecting bodies.
|
||||
internal ContactEdge _nodeA = new ContactEdge();
|
||||
internal ContactEdge _nodeB = new ContactEdge();
|
||||
internal int _toiCount;
|
||||
internal float _toi;
|
||||
|
||||
public Fixture FixtureA;
|
||||
public Fixture FixtureB;
|
||||
public float Friction { get; set; }
|
||||
public float Restitution { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the contact manifold. Do not modify the manifold unless you understand the
|
||||
/// internals of Box2D.
|
||||
/// </summary>
|
||||
public Manifold Manifold;
|
||||
|
||||
/// Get or set the desired tangent speed for a conveyor belt behavior. In meters per second.
|
||||
public float TangentSpeed { get; set; }
|
||||
|
||||
/// 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).
|
||||
/// NOTE: If you are setting Enabled to a constant true or false,
|
||||
/// use the explicit Enable() or Disable() functions instead to
|
||||
/// save the CPU from doing a branch operation.
|
||||
public bool Enabled { get; set; }
|
||||
|
||||
/// <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>
|
||||
/// Determines whether this contact is touching.
|
||||
/// </summary>
|
||||
/// <returns>
|
||||
/// <c>true</c> if this instance is touching; otherwise, <c>false</c>.
|
||||
/// </returns>
|
||||
public bool IsTouching { get; set; }
|
||||
|
||||
internal bool IslandFlag { get; set; }
|
||||
internal bool TOIFlag { get; set; }
|
||||
internal bool FilterFlag { get; set; }
|
||||
|
||||
public void ResetRestitution()
|
||||
{
|
||||
Restitution = Settings.MixRestitution(FixtureA.Restitution, FixtureB.Restitution);
|
||||
}
|
||||
|
||||
public void ResetFriction()
|
||||
{
|
||||
Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction);
|
||||
}
|
||||
|
||||
private Contact(Fixture fA, int indexA, Fixture fB, int indexB)
|
||||
{
|
||||
Reset(fA, indexA, fB, indexB);
|
||||
}
|
||||
|
||||
/// <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;
|
||||
|
||||
ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points);
|
||||
}
|
||||
|
||||
private void Reset(Fixture fA, int indexA, Fixture fB, int indexB)
|
||||
{
|
||||
Enabled = true;
|
||||
IsTouching = false;
|
||||
IslandFlag = false;
|
||||
FilterFlag = false;
|
||||
TOIFlag = false;
|
||||
|
||||
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;
|
||||
|
||||
//FPE: We only set the friction and restitution if we are not destroying the contact
|
||||
if (FixtureA != null && FixtureB != null)
|
||||
{
|
||||
Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction);
|
||||
Restitution = Settings.MixRestitution(FixtureA.Restitution, FixtureB.Restitution);
|
||||
}
|
||||
|
||||
TangentSpeed = 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)
|
||||
{
|
||||
Body bodyA = FixtureA.Body;
|
||||
Body bodyB = FixtureB.Body;
|
||||
|
||||
if (FixtureA == null || FixtureB == null)
|
||||
return;
|
||||
|
||||
Manifold oldManifold = Manifold;
|
||||
|
||||
// Re-enable this contact.
|
||||
Enabled = true;
|
||||
|
||||
bool touching;
|
||||
bool wasTouching = IsTouching;
|
||||
|
||||
bool sensor = FixtureA.IsSensor || FixtureB.IsSensor;
|
||||
|
||||
// Is this contact a sensor?
|
||||
if (sensor)
|
||||
{
|
||||
Shape shapeA = FixtureA.Shape;
|
||||
Shape shapeB = FixtureB.Shape;
|
||||
touching = Collision.Collision.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;
|
||||
|
||||
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;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Manifold.Points[i] = mp2;
|
||||
}
|
||||
|
||||
if (touching != wasTouching)
|
||||
{
|
||||
bodyA.Awake = true;
|
||||
bodyB.Awake = true;
|
||||
}
|
||||
}
|
||||
|
||||
IsTouching = touching;
|
||||
|
||||
if (wasTouching == false)
|
||||
{
|
||||
if (touching)
|
||||
{
|
||||
if (Settings.AllCollisionCallbacksAgree)
|
||||
{
|
||||
bool enabledA = true, enabledB = true;
|
||||
|
||||
// Report the collision to both participants. Track which ones returned true so we can
|
||||
// later call OnSeparation if the contact is disabled for a different reason.
|
||||
if (FixtureA.OnCollision != null)
|
||||
foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
|
||||
enabledA = handler(FixtureA, FixtureB, this) && enabledA;
|
||||
|
||||
// Reverse the order of the reported fixtures. The first fixture is always the one that the
|
||||
// user subscribed to.
|
||||
if (FixtureB.OnCollision != null)
|
||||
foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
|
||||
enabledB = handler(FixtureB, FixtureA, this) && enabledB;
|
||||
|
||||
Enabled = enabledA && enabledB;
|
||||
|
||||
// BeginContact can also return false and disable the contact
|
||||
if (enabledA && enabledB && contactManager.BeginContact != null)
|
||||
Enabled = contactManager.BeginContact(this);
|
||||
}
|
||||
else
|
||||
{
|
||||
//Report the collision to both participants:
|
||||
if (FixtureA.OnCollision != null)
|
||||
foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
|
||||
Enabled = handler(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)
|
||||
foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
|
||||
Enabled = handler(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) at any point by
|
||||
// any of the callbacks, we need to mark it as not touching and call any separation
|
||||
// callbacks for fixtures that didn't explicitly disable the collision.
|
||||
if (!Enabled)
|
||||
IsTouching = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (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.ChainAndCircle:
|
||||
ChainShape chain = (ChainShape)FixtureA.Shape;
|
||||
chain.GetChildEdge(_edge, ChildIndexA);
|
||||
Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB);
|
||||
break;
|
||||
case ContactType.ChainAndPolygon:
|
||||
ChainShape loop2 = (ChainShape)FixtureA.Shape;
|
||||
loop2.GetChildEdge(_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.Shape.ShapeType;
|
||||
ShapeType type2 = fixtureB.Shape.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()
|
||||
{
|
||||
#if USE_ACTIVE_CONTACT_SET
|
||||
FixtureA.Body.World.ContactManager.RemoveActiveContact(this);
|
||||
#endif
|
||||
FixtureA.Body._world._contactPool.Enqueue(this);
|
||||
|
||||
if (Manifold.PointCount > 0 && FixtureA.IsSensor == false && FixtureB.IsSensor == false)
|
||||
{
|
||||
FixtureA.Body.Awake = true;
|
||||
FixtureB.Body.Awake = true;
|
||||
}
|
||||
|
||||
Reset(null, 0, null, 0);
|
||||
}
|
||||
|
||||
#region Nested type: ContactType
|
||||
|
||||
private enum ContactType
|
||||
{
|
||||
NotSupported,
|
||||
Polygon,
|
||||
PolygonAndCircle,
|
||||
Circle,
|
||||
EdgeAndPolygon,
|
||||
EdgeAndCircle,
|
||||
ChainAndPolygon,
|
||||
ChainAndCircle,
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,979 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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 ContactPositionConstraint
|
||||
{
|
||||
public Vector2[] localPoints = new Vector2[Settings.MaxManifoldPoints];
|
||||
public Vector2 localNormal;
|
||||
public Vector2 localPoint;
|
||||
public int indexA;
|
||||
public int indexB;
|
||||
public float invMassA, invMassB;
|
||||
public Vector2 localCenterA, localCenterB;
|
||||
public float invIA, invIB;
|
||||
public ManifoldType type;
|
||||
public float radiusA, radiusB;
|
||||
public int pointCount;
|
||||
}
|
||||
|
||||
public sealed class VelocityConstraintPoint
|
||||
{
|
||||
public Vector2 rA;
|
||||
public Vector2 rB;
|
||||
public float normalImpulse;
|
||||
public float tangentImpulse;
|
||||
public float normalMass;
|
||||
public float tangentMass;
|
||||
public float velocityBias;
|
||||
}
|
||||
|
||||
public sealed class ContactVelocityConstraint
|
||||
{
|
||||
public VelocityConstraintPoint[] points = new VelocityConstraintPoint[Settings.MaxManifoldPoints];
|
||||
public Vector2 normal;
|
||||
public Mat22 normalMass;
|
||||
public Mat22 K;
|
||||
public int indexA;
|
||||
public int indexB;
|
||||
public float invMassA, invMassB;
|
||||
public float invIA, invIB;
|
||||
public float friction;
|
||||
public float restitution;
|
||||
public float tangentSpeed;
|
||||
public int pointCount;
|
||||
public int contactIndex;
|
||||
|
||||
public ContactVelocityConstraint()
|
||||
{
|
||||
for (int i = 0; i < Settings.MaxManifoldPoints; i++)
|
||||
{
|
||||
points[i] = new VelocityConstraintPoint();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public class ContactSolver
|
||||
{
|
||||
public TimeStep _step;
|
||||
public Position[] _positions;
|
||||
public Velocity[] _velocities;
|
||||
public ContactPositionConstraint[] _positionConstraints;
|
||||
public ContactVelocityConstraint[] _velocityConstraints;
|
||||
public Contact[] _contacts;
|
||||
public int _count;
|
||||
|
||||
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting)
|
||||
{
|
||||
_step = step;
|
||||
_count = count;
|
||||
_positions = positions;
|
||||
_velocities = velocities;
|
||||
_contacts = contacts;
|
||||
|
||||
// grow the array
|
||||
if (_velocityConstraints == null || _velocityConstraints.Length < count)
|
||||
{
|
||||
_velocityConstraints = new ContactVelocityConstraint[count * 2];
|
||||
_positionConstraints = new ContactPositionConstraint[count * 2];
|
||||
|
||||
for (int i = 0; i < _velocityConstraints.Length; i++)
|
||||
{
|
||||
_velocityConstraints[i] = new ContactVelocityConstraint();
|
||||
}
|
||||
|
||||
for (int i = 0; i < _positionConstraints.Length; i++)
|
||||
{
|
||||
_positionConstraints[i] = new ContactPositionConstraint();
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize position independent portions of the constraints.
|
||||
for (int i = 0; i < _count; ++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;
|
||||
|
||||
int pointCount = manifold.PointCount;
|
||||
Debug.Assert(pointCount > 0);
|
||||
|
||||
ContactVelocityConstraint vc = _velocityConstraints[i];
|
||||
vc.friction = contact.Friction;
|
||||
vc.restitution = contact.Restitution;
|
||||
vc.tangentSpeed = contact.TangentSpeed;
|
||||
vc.indexA = bodyA.IslandIndex;
|
||||
vc.indexB = bodyB.IslandIndex;
|
||||
vc.invMassA = bodyA._invMass;
|
||||
vc.invMassB = bodyB._invMass;
|
||||
vc.invIA = bodyA._invI;
|
||||
vc.invIB = bodyB._invI;
|
||||
vc.contactIndex = i;
|
||||
vc.pointCount = pointCount;
|
||||
vc.K.SetZero();
|
||||
vc.normalMass.SetZero();
|
||||
|
||||
ContactPositionConstraint pc = _positionConstraints[i];
|
||||
pc.indexA = bodyA.IslandIndex;
|
||||
pc.indexB = bodyB.IslandIndex;
|
||||
pc.invMassA = bodyA._invMass;
|
||||
pc.invMassB = bodyB._invMass;
|
||||
pc.localCenterA = bodyA._sweep.LocalCenter;
|
||||
pc.localCenterB = bodyB._sweep.LocalCenter;
|
||||
pc.invIA = bodyA._invI;
|
||||
pc.invIB = bodyB._invI;
|
||||
pc.localNormal = manifold.LocalNormal;
|
||||
pc.localPoint = manifold.LocalPoint;
|
||||
pc.pointCount = pointCount;
|
||||
pc.radiusA = radiusA;
|
||||
pc.radiusB = radiusB;
|
||||
pc.type = manifold.Type;
|
||||
|
||||
for (int j = 0; j < pointCount; ++j)
|
||||
{
|
||||
ManifoldPoint cp = manifold.Points[j];
|
||||
VelocityConstraintPoint vcp = vc.points[j];
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse;
|
||||
vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse;
|
||||
}
|
||||
else
|
||||
{
|
||||
vcp.normalImpulse = 0.0f;
|
||||
vcp.tangentImpulse = 0.0f;
|
||||
}
|
||||
|
||||
vcp.rA = Vector2.Zero;
|
||||
vcp.rB = Vector2.Zero;
|
||||
vcp.normalMass = 0.0f;
|
||||
vcp.tangentMass = 0.0f;
|
||||
vcp.velocityBias = 0.0f;
|
||||
|
||||
pc.localPoints[j] = cp.LocalPoint;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void InitializeVelocityConstraints()
|
||||
{
|
||||
for (int i = 0; i < _count; ++i)
|
||||
{
|
||||
ContactVelocityConstraint vc = _velocityConstraints[i];
|
||||
ContactPositionConstraint pc = _positionConstraints[i];
|
||||
|
||||
float radiusA = pc.radiusA;
|
||||
float radiusB = pc.radiusB;
|
||||
Manifold manifold = _contacts[vc.contactIndex].Manifold;
|
||||
|
||||
int indexA = vc.indexA;
|
||||
int indexB = vc.indexB;
|
||||
|
||||
float mA = vc.invMassA;
|
||||
float mB = vc.invMassB;
|
||||
float iA = vc.invIA;
|
||||
float iB = vc.invIB;
|
||||
Vector2 localCenterA = pc.localCenterA;
|
||||
Vector2 localCenterB = pc.localCenterB;
|
||||
|
||||
Vector2 cA = _positions[indexA].c;
|
||||
float aA = _positions[indexA].a;
|
||||
Vector2 vA = _velocities[indexA].v;
|
||||
float wA = _velocities[indexA].w;
|
||||
|
||||
Vector2 cB = _positions[indexB].c;
|
||||
float aB = _positions[indexB].a;
|
||||
Vector2 vB = _velocities[indexB].v;
|
||||
float wB = _velocities[indexB].w;
|
||||
|
||||
Debug.Assert(manifold.PointCount > 0);
|
||||
|
||||
Transform xfA = new Transform();
|
||||
Transform xfB = new Transform();
|
||||
xfA.q.Set(aA);
|
||||
xfB.q.Set(aB);
|
||||
xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
|
||||
xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
|
||||
|
||||
Vector2 normal;
|
||||
FixedArray2<Vector2> points;
|
||||
WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points);
|
||||
|
||||
vc.normal = normal;
|
||||
|
||||
int pointCount = vc.pointCount;
|
||||
for (int j = 0; j < pointCount; ++j)
|
||||
{
|
||||
VelocityConstraintPoint vcp = vc.points[j];
|
||||
|
||||
vcp.rA = points[j] - cA;
|
||||
vcp.rB = points[j] - cB;
|
||||
|
||||
float rnA = MathUtils.Cross(vcp.rA, vc.normal);
|
||||
float rnB = MathUtils.Cross(vcp.rB, vc.normal);
|
||||
|
||||
float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
||||
|
||||
vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
|
||||
|
||||
Vector2 tangent = MathUtils.Cross(vc.normal, 1.0f);
|
||||
|
||||
float rtA = MathUtils.Cross(vcp.rA, tangent);
|
||||
float rtB = MathUtils.Cross(vcp.rB, tangent);
|
||||
|
||||
float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
|
||||
|
||||
vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
|
||||
|
||||
// Setup a velocity bias for restitution.
|
||||
vcp.velocityBias = 0.0f;
|
||||
float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA));
|
||||
if (vRel < -Settings.VelocityThreshold)
|
||||
{
|
||||
vcp.velocityBias = -vc.restitution * vRel;
|
||||
}
|
||||
}
|
||||
|
||||
// If we have two points, then prepare the block solver.
|
||||
if (vc.pointCount == 2)
|
||||
{
|
||||
VelocityConstraintPoint vcp1 = vc.points[0];
|
||||
VelocityConstraintPoint vcp2 = vc.points[1];
|
||||
|
||||
float rn1A = MathUtils.Cross(vcp1.rA, vc.normal);
|
||||
float rn1B = MathUtils.Cross(vcp1.rB, vc.normal);
|
||||
float rn2A = MathUtils.Cross(vcp2.rA, vc.normal);
|
||||
float rn2B = MathUtils.Cross(vcp2.rB, vc.normal);
|
||||
|
||||
float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
|
||||
float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
|
||||
float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
|
||||
|
||||
// Ensure a reasonable condition number.
|
||||
const float k_maxConditionNumber = 1000.0f;
|
||||
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
|
||||
{
|
||||
// K is safe to invert.
|
||||
vc.K.ex = new Vector2(k11, k12);
|
||||
vc.K.ey = new Vector2(k12, k22);
|
||||
vc.normalMass = vc.K.Inverse;
|
||||
}
|
||||
else
|
||||
{
|
||||
// The constraints are redundant, just use one.
|
||||
// TODO_ERIN use deepest?
|
||||
vc.pointCount = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void WarmStart()
|
||||
{
|
||||
// Warm start.
|
||||
for (int i = 0; i < _count; ++i)
|
||||
{
|
||||
ContactVelocityConstraint vc = _velocityConstraints[i];
|
||||
|
||||
int indexA = vc.indexA;
|
||||
int indexB = vc.indexB;
|
||||
float mA = vc.invMassA;
|
||||
float iA = vc.invIA;
|
||||
float mB = vc.invMassB;
|
||||
float iB = vc.invIB;
|
||||
int pointCount = vc.pointCount;
|
||||
|
||||
Vector2 vA = _velocities[indexA].v;
|
||||
float wA = _velocities[indexA].w;
|
||||
Vector2 vB = _velocities[indexB].v;
|
||||
float wB = _velocities[indexB].w;
|
||||
|
||||
Vector2 normal = vc.normal;
|
||||
Vector2 tangent = MathUtils.Cross(normal, 1.0f);
|
||||
|
||||
for (int j = 0; j < pointCount; ++j)
|
||||
{
|
||||
VelocityConstraintPoint vcp = vc.points[j];
|
||||
Vector2 P = vcp.normalImpulse * normal + vcp.tangentImpulse * tangent;
|
||||
wA -= iA * MathUtils.Cross(vcp.rA, P);
|
||||
vA -= mA * P;
|
||||
wB += iB * MathUtils.Cross(vcp.rB, P);
|
||||
vB += mB * P;
|
||||
}
|
||||
|
||||
_velocities[indexA].v = vA;
|
||||
_velocities[indexA].w = wA;
|
||||
_velocities[indexB].v = vB;
|
||||
_velocities[indexB].w = wB;
|
||||
}
|
||||
}
|
||||
|
||||
public void SolveVelocityConstraints()
|
||||
{
|
||||
for (int i = 0; i < _count; ++i)
|
||||
{
|
||||
ContactVelocityConstraint vc = _velocityConstraints[i];
|
||||
|
||||
int indexA = vc.indexA;
|
||||
int indexB = vc.indexB;
|
||||
float mA = vc.invMassA;
|
||||
float iA = vc.invIA;
|
||||
float mB = vc.invMassB;
|
||||
float iB = vc.invIB;
|
||||
int pointCount = vc.pointCount;
|
||||
|
||||
Vector2 vA = _velocities[indexA].v;
|
||||
float wA = _velocities[indexA].w;
|
||||
Vector2 vB = _velocities[indexB].v;
|
||||
float wB = _velocities[indexB].w;
|
||||
|
||||
Vector2 normal = vc.normal;
|
||||
Vector2 tangent = MathUtils.Cross(normal, 1.0f);
|
||||
float friction = vc.friction;
|
||||
|
||||
Debug.Assert(pointCount == 1 || pointCount == 2);
|
||||
|
||||
// Solve tangent constraints first because non-penetration is more important
|
||||
// than friction.
|
||||
for (int j = 0; j < pointCount; ++j)
|
||||
{
|
||||
VelocityConstraintPoint vcp = vc.points[j];
|
||||
|
||||
// Relative velocity at contact
|
||||
Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
|
||||
|
||||
// Compute tangent force
|
||||
float vt = Vector2.Dot(dv, tangent) - vc.tangentSpeed;
|
||||
float lambda = vcp.tangentMass * (-vt);
|
||||
|
||||
// b2Clamp the accumulated force
|
||||
float maxFriction = friction * vcp.normalImpulse;
|
||||
float newImpulse = MathUtils.Clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
|
||||
lambda = newImpulse - vcp.tangentImpulse;
|
||||
vcp.tangentImpulse = newImpulse;
|
||||
|
||||
// Apply contact impulse
|
||||
Vector2 P = lambda * tangent;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * MathUtils.Cross(vcp.rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * MathUtils.Cross(vcp.rB, P);
|
||||
}
|
||||
|
||||
// Solve normal constraints
|
||||
if (vc.pointCount == 1)
|
||||
{
|
||||
VelocityConstraintPoint vcp = vc.points[0];
|
||||
|
||||
// Relative velocity at contact
|
||||
Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
|
||||
|
||||
// Compute normal impulse
|
||||
float vn = Vector2.Dot(dv, normal);
|
||||
float lambda = -vcp.normalMass * (vn - vcp.velocityBias);
|
||||
|
||||
// b2Clamp the accumulated impulse
|
||||
float newImpulse = Math.Max(vcp.normalImpulse + lambda, 0.0f);
|
||||
lambda = newImpulse - vcp.normalImpulse;
|
||||
vcp.normalImpulse = newImpulse;
|
||||
|
||||
// Apply contact impulse
|
||||
Vector2 P = lambda * normal;
|
||||
vA -= mA * P;
|
||||
wA -= iA * MathUtils.Cross(vcp.rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * MathUtils.Cross(vcp.rB, P);
|
||||
}
|
||||
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 = vn0 - 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 = a + d
|
||||
//
|
||||
// a := old total impulse
|
||||
// x := new total impulse
|
||||
// d := incremental impulse
|
||||
//
|
||||
// For the current iteration we extend the formula for the incremental impulse
|
||||
// to compute the new total impulse:
|
||||
//
|
||||
// vn = A * d + b
|
||||
// = A * (x - a) + b
|
||||
// = A * x + b - A * a
|
||||
// = A * x + b'
|
||||
// b' = b - A * a;
|
||||
|
||||
VelocityConstraintPoint cp1 = vc.points[0];
|
||||
VelocityConstraintPoint cp2 = vc.points[1];
|
||||
|
||||
Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse);
|
||||
Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
|
||||
|
||||
// Relative velocity at contact
|
||||
Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
|
||||
Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
|
||||
|
||||
// Compute normal velocity
|
||||
float vn1 = Vector2.Dot(dv1, normal);
|
||||
float vn2 = Vector2.Dot(dv2, normal);
|
||||
|
||||
Vector2 b = new Vector2();
|
||||
b.X = vn1 - cp1.velocityBias;
|
||||
b.Y = vn2 - cp2.velocityBias;
|
||||
|
||||
// Compute b'
|
||||
b -= MathUtils.Mul(ref vc.K, a);
|
||||
|
||||
const float k_errorTol = 1e-3f;
|
||||
//B2_NOT_USED(k_errorTol);
|
||||
|
||||
for (; ; )
|
||||
{
|
||||
//
|
||||
// Case 1: vn = 0
|
||||
//
|
||||
// 0 = A * x + b'
|
||||
//
|
||||
// Solve for x:
|
||||
//
|
||||
// x = - inv(A) * b'
|
||||
//
|
||||
Vector2 x = -MathUtils.Mul(ref vc.normalMass, b);
|
||||
|
||||
if (x.X >= 0.0f && x.Y >= 0.0f)
|
||||
{
|
||||
// Get the incremental impulse
|
||||
Vector2 d = x - a;
|
||||
|
||||
// Apply incremental impulse
|
||||
Vector2 P1 = d.X * normal;
|
||||
Vector2 P2 = d.Y * normal;
|
||||
vA -= mA * (P1 + P2);
|
||||
wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
|
||||
|
||||
vB += mB * (P1 + P2);
|
||||
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.normalImpulse = x.X;
|
||||
cp2.normalImpulse = x.Y;
|
||||
|
||||
#if B2_DEBUG_SOLVER
|
||||
// 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);
|
||||
|
||||
b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol);
|
||||
b2Assert(b2Abs(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'
|
||||
//
|
||||
x.X = -cp1.normalMass * b.X;
|
||||
x.Y = 0.0f;
|
||||
vn1 = 0.0f;
|
||||
vn2 = vc.K.ex.Y * x.X + b.Y;
|
||||
|
||||
if (x.X >= 0.0f && vn2 >= 0.0f)
|
||||
{
|
||||
// Get the incremental impulse
|
||||
Vector2 d = x - a;
|
||||
|
||||
// Apply incremental impulse
|
||||
Vector2 P1 = d.X * normal;
|
||||
Vector2 P2 = d.Y * normal;
|
||||
vA -= mA * (P1 + P2);
|
||||
wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
|
||||
|
||||
vB += mB * (P1 + P2);
|
||||
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.normalImpulse = x.X;
|
||||
cp2.normalImpulse = x.Y;
|
||||
|
||||
#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);
|
||||
|
||||
b2Assert(b2Abs(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'
|
||||
//
|
||||
x.X = 0.0f;
|
||||
x.Y = -cp2.normalMass * b.Y;
|
||||
vn1 = vc.K.ey.X * x.Y + b.X;
|
||||
vn2 = 0.0f;
|
||||
|
||||
if (x.Y >= 0.0f && vn1 >= 0.0f)
|
||||
{
|
||||
// Resubstitute for the incremental impulse
|
||||
Vector2 d = x - a;
|
||||
|
||||
// Apply incremental impulse
|
||||
Vector2 P1 = d.X * normal;
|
||||
Vector2 P2 = d.Y * normal;
|
||||
vA -= mA * (P1 + P2);
|
||||
wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
|
||||
|
||||
vB += mB * (P1 + P2);
|
||||
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.normalImpulse = x.X;
|
||||
cp2.normalImpulse = x.Y;
|
||||
|
||||
#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);
|
||||
|
||||
b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
//
|
||||
// Case 4: x1 = 0 and x2 = 0
|
||||
//
|
||||
// vn1 = b1
|
||||
// vn2 = b2;
|
||||
x.X = 0.0f;
|
||||
x.Y = 0.0f;
|
||||
vn1 = b.X;
|
||||
vn2 = b.Y;
|
||||
|
||||
if (vn1 >= 0.0f && vn2 >= 0.0f)
|
||||
{
|
||||
// Resubstitute for the incremental impulse
|
||||
Vector2 d = x - a;
|
||||
|
||||
// Apply incremental impulse
|
||||
Vector2 P1 = d.X * normal;
|
||||
Vector2 P2 = d.Y * normal;
|
||||
vA -= mA * (P1 + P2);
|
||||
wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
|
||||
|
||||
vB += mB * (P1 + P2);
|
||||
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.normalImpulse = x.X;
|
||||
cp2.normalImpulse = x.Y;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_velocities[indexA].v = vA;
|
||||
_velocities[indexA].w = wA;
|
||||
_velocities[indexB].v = vB;
|
||||
_velocities[indexB].w = wB;
|
||||
}
|
||||
}
|
||||
|
||||
public void StoreImpulses()
|
||||
{
|
||||
for (int i = 0; i < _count; ++i)
|
||||
{
|
||||
ContactVelocityConstraint vc = _velocityConstraints[i];
|
||||
Manifold manifold = _contacts[vc.contactIndex].Manifold;
|
||||
|
||||
for (int j = 0; j < vc.pointCount; ++j)
|
||||
{
|
||||
ManifoldPoint point = manifold.Points[j];
|
||||
point.NormalImpulse = vc.points[j].normalImpulse;
|
||||
point.TangentImpulse = vc.points[j].tangentImpulse;
|
||||
manifold.Points[j] = point;
|
||||
}
|
||||
|
||||
_contacts[vc.contactIndex].Manifold = manifold;
|
||||
}
|
||||
}
|
||||
|
||||
public bool SolvePositionConstraints()
|
||||
{
|
||||
float minSeparation = 0.0f;
|
||||
|
||||
for (int i = 0; i < _count; ++i)
|
||||
{
|
||||
ContactPositionConstraint pc = _positionConstraints[i];
|
||||
|
||||
int indexA = pc.indexA;
|
||||
int indexB = pc.indexB;
|
||||
Vector2 localCenterA = pc.localCenterA;
|
||||
float mA = pc.invMassA;
|
||||
float iA = pc.invIA;
|
||||
Vector2 localCenterB = pc.localCenterB;
|
||||
float mB = pc.invMassB;
|
||||
float iB = pc.invIB;
|
||||
int pointCount = pc.pointCount;
|
||||
|
||||
Vector2 cA = _positions[indexA].c;
|
||||
float aA = _positions[indexA].a;
|
||||
|
||||
Vector2 cB = _positions[indexB].c;
|
||||
float aB = _positions[indexB].a;
|
||||
|
||||
// Solve normal constraints
|
||||
for (int j = 0; j < pointCount; ++j)
|
||||
{
|
||||
Transform xfA = new Transform();
|
||||
Transform xfB = new Transform();
|
||||
xfA.q.Set(aA);
|
||||
xfB.q.Set(aB);
|
||||
xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
|
||||
xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
|
||||
|
||||
Vector2 normal;
|
||||
Vector2 point;
|
||||
float separation;
|
||||
|
||||
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
|
||||
|
||||
Vector2 rA = point - cA;
|
||||
Vector2 rB = point - cB;
|
||||
|
||||
// Track max constraint error.
|
||||
minSeparation = Math.Min(minSeparation, separation);
|
||||
|
||||
// Prevent large corrections and allow slop.
|
||||
float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
|
||||
|
||||
// Compute the effective mass.
|
||||
float rnA = MathUtils.Cross(rA, normal);
|
||||
float rnB = MathUtils.Cross(rB, normal);
|
||||
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
||||
|
||||
// Compute normal impulse
|
||||
float impulse = K > 0.0f ? -C / K : 0.0f;
|
||||
|
||||
Vector2 P = impulse * normal;
|
||||
|
||||
cA -= mA * P;
|
||||
aA -= iA * MathUtils.Cross(rA, P);
|
||||
|
||||
cB += mB * P;
|
||||
aB += iB * MathUtils.Cross(rB, P);
|
||||
}
|
||||
|
||||
_positions[indexA].c = cA;
|
||||
_positions[indexA].a = aA;
|
||||
|
||||
_positions[indexB].c = cB;
|
||||
_positions[indexB].a = aB;
|
||||
}
|
||||
|
||||
// We can't expect minSpeparation >= -b2_linearSlop because we don't
|
||||
// push the separation above -b2_linearSlop.
|
||||
return minSeparation >= -3.0f * Settings.LinearSlop;
|
||||
}
|
||||
|
||||
// Sequential position solver for position constraints.
|
||||
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
|
||||
{
|
||||
float minSeparation = 0.0f;
|
||||
|
||||
for (int i = 0; i < _count; ++i)
|
||||
{
|
||||
ContactPositionConstraint pc = _positionConstraints[i];
|
||||
|
||||
int indexA = pc.indexA;
|
||||
int indexB = pc.indexB;
|
||||
Vector2 localCenterA = pc.localCenterA;
|
||||
Vector2 localCenterB = pc.localCenterB;
|
||||
int pointCount = pc.pointCount;
|
||||
|
||||
float mA = 0.0f;
|
||||
float iA = 0.0f;
|
||||
if (indexA == toiIndexA || indexA == toiIndexB)
|
||||
{
|
||||
mA = pc.invMassA;
|
||||
iA = pc.invIA;
|
||||
}
|
||||
|
||||
float mB = 0.0f;
|
||||
float iB = 0.0f;
|
||||
if (indexB == toiIndexA || indexB == toiIndexB)
|
||||
{
|
||||
mB = pc.invMassB;
|
||||
iB = pc.invIB;
|
||||
}
|
||||
|
||||
Vector2 cA = _positions[indexA].c;
|
||||
float aA = _positions[indexA].a;
|
||||
|
||||
Vector2 cB = _positions[indexB].c;
|
||||
float aB = _positions[indexB].a;
|
||||
|
||||
// Solve normal constraints
|
||||
for (int j = 0; j < pointCount; ++j)
|
||||
{
|
||||
Transform xfA = new Transform();
|
||||
Transform xfB = new Transform();
|
||||
xfA.q.Set(aA);
|
||||
xfB.q.Set(aB);
|
||||
xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
|
||||
xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
|
||||
|
||||
Vector2 normal;
|
||||
Vector2 point;
|
||||
float separation;
|
||||
|
||||
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
|
||||
|
||||
Vector2 rA = point - cA;
|
||||
Vector2 rB = point - cB;
|
||||
|
||||
// Track max constraint error.
|
||||
minSeparation = Math.Min(minSeparation, separation);
|
||||
|
||||
// Prevent large corrections and allow slop.
|
||||
float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
|
||||
|
||||
// Compute the effective mass.
|
||||
float rnA = MathUtils.Cross(rA, normal);
|
||||
float rnB = MathUtils.Cross(rB, normal);
|
||||
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
||||
|
||||
// Compute normal impulse
|
||||
float impulse = K > 0.0f ? -C / K : 0.0f;
|
||||
|
||||
Vector2 P = impulse * normal;
|
||||
|
||||
cA -= mA * P;
|
||||
aA -= iA * MathUtils.Cross(rA, P);
|
||||
|
||||
cB += mB * P;
|
||||
aB += iB * MathUtils.Cross(rB, P);
|
||||
}
|
||||
|
||||
_positions[indexA].c = cA;
|
||||
_positions[indexA].a = aA;
|
||||
|
||||
_positions[indexB].c = cB;
|
||||
_positions[indexB].a = aB;
|
||||
}
|
||||
|
||||
// We can't expect minSpeparation >= -b2_linearSlop because we don't
|
||||
// push the separation above -b2_linearSlop.
|
||||
return minSeparation >= -1.5f * Settings.LinearSlop;
|
||||
}
|
||||
|
||||
public static class WorldManifold
|
||||
{
|
||||
/// <summary>
|
||||
/// Evaluate the manifold with supplied transforms. This assumes
|
||||
/// modest motion from the original state. This does not change the
|
||||
/// point count, impulses, etc. The radii must come from the Shapes
|
||||
/// that generated the manifold.
|
||||
/// </summary>
|
||||
/// <param name="manifold">The manifold.</param>
|
||||
/// <param name="xfA">The transform for A.</param>
|
||||
/// <param name="radiusA">The radius for A.</param>
|
||||
/// <param name="xfB">The transform for B.</param>
|
||||
/// <param name="radiusB">The radius for B.</param>
|
||||
/// <param name="normal">World vector pointing from A to B</param>
|
||||
/// <param name="points">Torld contact point (point of intersection).</param>
|
||||
public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2<Vector2> points)
|
||||
{
|
||||
normal = Vector2.Zero;
|
||||
points = new FixedArray2<Vector2>();
|
||||
|
||||
if (manifold.PointCount == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
switch (manifold.Type)
|
||||
{
|
||||
case ManifoldType.Circles:
|
||||
{
|
||||
normal = new Vector2(1.0f, 0.0f);
|
||||
Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
|
||||
Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
|
||||
if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
|
||||
{
|
||||
normal = pointB - pointA;
|
||||
normal.Normalize();
|
||||
}
|
||||
|
||||
Vector2 cA = pointA + radiusA * normal;
|
||||
Vector2 cB = pointB - radiusB * normal;
|
||||
points[0] = 0.5f * (cA + cB);
|
||||
}
|
||||
break;
|
||||
|
||||
case ManifoldType.FaceA:
|
||||
{
|
||||
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
|
||||
Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
|
||||
|
||||
for (int i = 0; i < manifold.PointCount; ++i)
|
||||
{
|
||||
Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
|
||||
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
|
||||
Vector2 cB = clipPoint - radiusB * normal;
|
||||
points[i] = 0.5f * (cA + cB);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ManifoldType.FaceB:
|
||||
{
|
||||
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
|
||||
Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
|
||||
|
||||
for (int i = 0; i < manifold.PointCount; ++i)
|
||||
{
|
||||
Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
|
||||
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
|
||||
Vector2 cA = clipPoint - radiusA * normal;
|
||||
points[i] = 0.5f * (cA + cB);
|
||||
}
|
||||
|
||||
// Ensure normal points from A to B.
|
||||
normal = -normal;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static class PositionSolverManifold
|
||||
{
|
||||
public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation)
|
||||
{
|
||||
Debug.Assert(pc.pointCount > 0);
|
||||
|
||||
|
||||
switch (pc.type)
|
||||
{
|
||||
case ManifoldType.Circles:
|
||||
{
|
||||
Vector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint);
|
||||
Vector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]);
|
||||
normal = pointB - pointA;
|
||||
normal.Normalize();
|
||||
point = 0.5f * (pointA + pointB);
|
||||
separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
|
||||
}
|
||||
break;
|
||||
|
||||
case ManifoldType.FaceA:
|
||||
{
|
||||
normal = MathUtils.Mul(xfA.q, pc.localNormal);
|
||||
Vector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);
|
||||
|
||||
Vector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
|
||||
separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
|
||||
point = clipPoint;
|
||||
}
|
||||
break;
|
||||
|
||||
case ManifoldType.FaceB:
|
||||
{
|
||||
normal = MathUtils.Mul(xfB.q, pc.localNormal);
|
||||
Vector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);
|
||||
|
||||
Vector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
|
||||
separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
|
||||
point = clipPoint;
|
||||
|
||||
// Ensure normal points from A to B
|
||||
normal = -normal;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
normal = Vector2.Zero;
|
||||
point = Vector2.Zero;
|
||||
separation = 0;
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,619 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
//#define USE_IGNORE_CCD_CATEGORIES
|
||||
|
||||
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
|
||||
{
|
||||
[ThreadStatic]
|
||||
private static int _fixtureIdCounter;
|
||||
private bool _isSensor;
|
||||
private float _friction;
|
||||
private float _restitution;
|
||||
|
||||
internal Category _collidesWith;
|
||||
internal Category _collisionCategories;
|
||||
internal short _collisionGroup;
|
||||
internal HashSet<int> _collisionIgnores;
|
||||
|
||||
public FixtureProxy[] Proxies;
|
||||
public int ProxyCount;
|
||||
public Category IgnoreCCDWith;
|
||||
|
||||
/// <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: This can in some cases be called multiple times, as a fixture can have multiple contacts.
|
||||
/// Note The first fixture argument is always the fixture that the delegate is subscribed to.
|
||||
/// </summary>
|
||||
public OnSeparationEventHandler OnSeparation;
|
||||
|
||||
internal Fixture()
|
||||
{
|
||||
FixtureId = _fixtureIdCounter++;
|
||||
|
||||
_collisionCategories = Settings.DefaultFixtureCollisionCategories;
|
||||
_collidesWith = Settings.DefaultFixtureCollidesWith;
|
||||
_collisionGroup = 0;
|
||||
_collisionIgnores = new HashSet<int>();
|
||||
|
||||
IgnoreCCDWith = Settings.DefaultFixtureIgnoreCCDWith;
|
||||
|
||||
//Fixture defaults
|
||||
Friction = 0.2f;
|
||||
Restitution = 0;
|
||||
}
|
||||
|
||||
internal Fixture(Body body, Shape shape, object userData = null)
|
||||
: this()
|
||||
{
|
||||
#if DEBUG
|
||||
if (shape.ShapeType == ShapeType.Polygon)
|
||||
((PolygonShape)shape).Vertices.AttachedToBody = true;
|
||||
#endif
|
||||
|
||||
Body = body;
|
||||
UserData = userData;
|
||||
Shape = shape.Clone();
|
||||
|
||||
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 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 { return _isSensor; }
|
||||
set
|
||||
{
|
||||
if (Body != null)
|
||||
Body.Awake = true;
|
||||
|
||||
_isSensor = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <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>
|
||||
/// Set the coefficient of friction. This will _not_ change the friction of
|
||||
/// existing contacts.
|
||||
/// </summary>
|
||||
/// <value>The friction.</value>
|
||||
public float Friction
|
||||
{
|
||||
get { return _friction; }
|
||||
set
|
||||
{
|
||||
Debug.Assert(!float.IsNaN(value));
|
||||
|
||||
_friction = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Set the coefficient of restitution. This will not change the restitution of
|
||||
/// existing contacts.
|
||||
/// </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; internal 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.Contains(fixture.FixtureId))
|
||||
{
|
||||
_collisionIgnores.Remove(fixture.FixtureId);
|
||||
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.Contains(fixture.FixtureId))
|
||||
{
|
||||
_collisionIgnores.Add(fixture.FixtureId);
|
||||
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)
|
||||
{
|
||||
return _collisionIgnores.Contains(fixture.FixtureId);
|
||||
}
|
||||
|
||||
/// <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>
|
||||
private 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.FilterFlag = true;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
if (Body.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._worldHasNewFixture = true;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
internal void Destroy()
|
||||
{
|
||||
#if DEBUG
|
||||
if (Shape.ShapeType == ShapeType.Polygon)
|
||||
((PolygonShape)Shape).Vertices.AttachedToBody = false;
|
||||
#endif
|
||||
|
||||
// The proxies must be destroyed before calling this.
|
||||
Debug.Assert(ProxyCount == 0);
|
||||
|
||||
// Free the proxy array.
|
||||
Proxies = null;
|
||||
Shape = null;
|
||||
|
||||
//FPE: We set the userdata to null here to help prevent bugs related to stale references in GC
|
||||
UserData = 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;
|
||||
|
||||
//FPE note: This line needs to be after the previous two because FixtureProxy is a struct
|
||||
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.p - transform1.p;
|
||||
|
||||
broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Only compares the values of this fixture, and not the attached shape or body.
|
||||
/// This is used for deduplication in serialization only.
|
||||
/// </summary>
|
||||
internal bool CompareTo(Fixture fixture)
|
||||
{
|
||||
return (_collidesWith == fixture._collidesWith &&
|
||||
_collisionCategories == fixture._collisionCategories &&
|
||||
_collisionGroup == fixture._collisionGroup &&
|
||||
Friction == fixture.Friction &&
|
||||
IsSensor == fixture.IsSensor &&
|
||||
Restitution == fixture.Restitution &&
|
||||
UserData == fixture.UserData &&
|
||||
IgnoreCCDWith == fixture.IgnoreCCDWith &&
|
||||
SequenceEqual(_collisionIgnores, fixture._collisionIgnores));
|
||||
}
|
||||
|
||||
private bool SequenceEqual<T>(HashSet<T> first, HashSet<T> second)
|
||||
{
|
||||
if (first.Count != second.Count)
|
||||
return false;
|
||||
|
||||
using (IEnumerator<T> enumerator1 = first.GetEnumerator())
|
||||
{
|
||||
using (IEnumerator<T> enumerator2 = second.GetEnumerator())
|
||||
{
|
||||
while (enumerator1.MoveNext())
|
||||
{
|
||||
if (!enumerator2.MoveNext() || !Equals(enumerator1.Current, enumerator2.Current))
|
||||
return false;
|
||||
}
|
||||
|
||||
if (enumerator2.MoveNext())
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clones the fixture and attached shape onto the specified body.
|
||||
/// </summary>
|
||||
/// <param name="body">The body you wish to clone the fixture onto.</param>
|
||||
/// <returns>The cloned fixture.</returns>
|
||||
public Fixture CloneOnto(Body body)
|
||||
{
|
||||
Fixture fixture = new Fixture();
|
||||
fixture.Body = body;
|
||||
fixture.Shape = Shape.Clone();
|
||||
fixture.UserData = UserData;
|
||||
fixture.Restitution = Restitution;
|
||||
fixture.Friction = Friction;
|
||||
fixture.IsSensor = IsSensor;
|
||||
fixture._collisionGroup = _collisionGroup;
|
||||
fixture._collisionCategories = _collisionCategories;
|
||||
fixture._collidesWith = _collidesWith;
|
||||
fixture.IgnoreCCDWith = IgnoreCCDWith;
|
||||
|
||||
foreach (int ignore in _collisionIgnores)
|
||||
{
|
||||
fixture._collisionIgnores.Add(ignore);
|
||||
}
|
||||
|
||||
fixture.RegisterFixture();
|
||||
return fixture;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,449 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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
|
||||
{
|
||||
private ContactManager _contactManager;
|
||||
private ContactSolver _contactSolver = new ContactSolver();
|
||||
private Contact[] _contacts;
|
||||
private Joint[] _joints;
|
||||
|
||||
private const float LinTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
|
||||
private const float AngTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
|
||||
private Stopwatch _watch = new Stopwatch();
|
||||
|
||||
public Body[] Bodies;
|
||||
public int BodyCount;
|
||||
public int ContactCount;
|
||||
public int JointCount;
|
||||
|
||||
public Velocity[] _velocities;
|
||||
public Position[] _positions;
|
||||
|
||||
public int BodyCapacity;
|
||||
public int ContactCapacity;
|
||||
public int JointCapacity;
|
||||
public float JointUpdateTime;
|
||||
|
||||
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];
|
||||
_velocities = new Velocity[bodyCapacity];
|
||||
_positions = new Position[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;
|
||||
}
|
||||
|
||||
public void Solve(ref TimeStep step, ref Vector2 gravity)
|
||||
{
|
||||
float h = step.dt;
|
||||
|
||||
// Integrate velocities and apply damping. Initialize the body state.
|
||||
for (int i = 0; i < BodyCount; ++i)
|
||||
{
|
||||
Body b = Bodies[i];
|
||||
|
||||
Vector2 c = b._sweep.C;
|
||||
float a = b._sweep.A;
|
||||
Vector2 v = b._linearVelocity;
|
||||
float w = b._angularVelocity;
|
||||
|
||||
// Store positions for continuous collision.
|
||||
b._sweep.C0 = b._sweep.C;
|
||||
b._sweep.A0 = b._sweep.A;
|
||||
|
||||
if (b.BodyType == BodyType.Dynamic)
|
||||
{
|
||||
// Integrate velocities.
|
||||
|
||||
// FPE: Only apply gravity if the body wants it.
|
||||
if (b.IgnoreGravity)
|
||||
v += h * (b._invMass * b._force);
|
||||
else
|
||||
v += h * (b.GravityScale * gravity + b._invMass * b._force);
|
||||
|
||||
w += h * 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
|
||||
v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
|
||||
w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
_positions[i].c = c;
|
||||
_positions[i].a = a;
|
||||
_velocities[i].v = v;
|
||||
_velocities[i].w = w;
|
||||
}
|
||||
|
||||
// Solver data
|
||||
SolverData solverData = new SolverData();
|
||||
solverData.step = step;
|
||||
solverData.positions = _positions;
|
||||
solverData.velocities = _velocities;
|
||||
|
||||
_contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
|
||||
_contactSolver.InitializeVelocityConstraints();
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
_contactSolver.WarmStart();
|
||||
}
|
||||
|
||||
if (Settings.EnableDiagnostics)
|
||||
_watch.Start();
|
||||
|
||||
for (int i = 0; i < JointCount; ++i)
|
||||
{
|
||||
if (_joints[i].Enabled)
|
||||
_joints[i].InitVelocityConstraints(ref solverData);
|
||||
}
|
||||
|
||||
if (Settings.EnableDiagnostics)
|
||||
_watch.Stop();
|
||||
|
||||
// Solve velocity constraints.
|
||||
for (int i = 0; i < Settings.VelocityIterations; ++i)
|
||||
{
|
||||
for (int j = 0; j < JointCount; ++j)
|
||||
{
|
||||
Joint joint = _joints[j];
|
||||
|
||||
if (!joint.Enabled)
|
||||
continue;
|
||||
|
||||
if (Settings.EnableDiagnostics)
|
||||
_watch.Start();
|
||||
|
||||
joint.SolveVelocityConstraints(ref solverData);
|
||||
joint.Validate(step.inv_dt);
|
||||
|
||||
if (Settings.EnableDiagnostics)
|
||||
_watch.Stop();
|
||||
}
|
||||
|
||||
_contactSolver.SolveVelocityConstraints();
|
||||
}
|
||||
|
||||
// Store impulses for warm starting.
|
||||
_contactSolver.StoreImpulses();
|
||||
|
||||
// Integrate positions
|
||||
for (int i = 0; i < BodyCount; ++i)
|
||||
{
|
||||
Vector2 c = _positions[i].c;
|
||||
float a = _positions[i].a;
|
||||
Vector2 v = _velocities[i].v;
|
||||
float w = _velocities[i].w;
|
||||
|
||||
// Check for large velocities
|
||||
Vector2 translation = h * v;
|
||||
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
|
||||
{
|
||||
float ratio = Settings.MaxTranslation / translation.Length();
|
||||
v *= ratio;
|
||||
}
|
||||
|
||||
float rotation = h * w;
|
||||
if (rotation * rotation > Settings.MaxRotationSquared)
|
||||
{
|
||||
float ratio = Settings.MaxRotation / Math.Abs(rotation);
|
||||
w *= ratio;
|
||||
}
|
||||
|
||||
// Integrate
|
||||
c += h * v;
|
||||
a += h * w;
|
||||
|
||||
_positions[i].c = c;
|
||||
_positions[i].a = a;
|
||||
_velocities[i].v = v;
|
||||
_velocities[i].w = w;
|
||||
}
|
||||
|
||||
|
||||
// Solve position constraints
|
||||
bool positionSolved = false;
|
||||
for (int i = 0; i < Settings.PositionIterations; ++i)
|
||||
{
|
||||
bool contactsOkay = _contactSolver.SolvePositionConstraints();
|
||||
|
||||
bool jointsOkay = true;
|
||||
for (int j = 0; j < JointCount; ++j)
|
||||
{
|
||||
Joint joint = _joints[j];
|
||||
|
||||
if (!joint.Enabled)
|
||||
continue;
|
||||
|
||||
if (Settings.EnableDiagnostics)
|
||||
_watch.Start();
|
||||
|
||||
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
|
||||
|
||||
if (Settings.EnableDiagnostics)
|
||||
_watch.Stop();
|
||||
|
||||
jointsOkay = jointsOkay && jointOkay;
|
||||
}
|
||||
|
||||
if (contactsOkay && jointsOkay)
|
||||
{
|
||||
// Exit early if the position errors are small.
|
||||
positionSolved = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (Settings.EnableDiagnostics)
|
||||
{
|
||||
JointUpdateTime = _watch.ElapsedTicks;
|
||||
_watch.Reset();
|
||||
}
|
||||
|
||||
// Copy state buffers back to the bodies
|
||||
for (int i = 0; i < BodyCount; ++i)
|
||||
{
|
||||
Body body = Bodies[i];
|
||||
body._sweep.C = _positions[i].c;
|
||||
body._sweep.A = _positions[i].a;
|
||||
body._linearVelocity = _velocities[i].v;
|
||||
body._angularVelocity = _velocities[i].w;
|
||||
body.SynchronizeTransform();
|
||||
}
|
||||
|
||||
Report(_contactSolver._velocityConstraints);
|
||||
|
||||
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.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
|
||||
{
|
||||
b._sleepTime = 0.0f;
|
||||
minSleepTime = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
b._sleepTime += h;
|
||||
minSleepTime = Math.Min(minSleepTime, b._sleepTime);
|
||||
}
|
||||
}
|
||||
|
||||
if (minSleepTime >= Settings.TimeToSleep && positionSolved)
|
||||
{
|
||||
for (int i = 0; i < BodyCount; ++i)
|
||||
{
|
||||
Body b = Bodies[i];
|
||||
b.Awake = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
|
||||
{
|
||||
Debug.Assert(toiIndexA < BodyCount);
|
||||
Debug.Assert(toiIndexB < BodyCount);
|
||||
|
||||
// Initialize the body state.
|
||||
for (int i = 0; i < BodyCount; ++i)
|
||||
{
|
||||
Body b = Bodies[i];
|
||||
_positions[i].c = b._sweep.C;
|
||||
_positions[i].a = b._sweep.A;
|
||||
_velocities[i].v = b._linearVelocity;
|
||||
_velocities[i].w = b._angularVelocity;
|
||||
}
|
||||
|
||||
_contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting);
|
||||
|
||||
// Solve position constraints.
|
||||
for (int i = 0; i < Settings.TOIPositionIterations; ++i)
|
||||
{
|
||||
bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
|
||||
if (contactsOkay)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Leap of faith to new safe state.
|
||||
Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c;
|
||||
Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a;
|
||||
Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c;
|
||||
Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a;
|
||||
|
||||
// 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.
|
||||
|
||||
float h = subStep.dt;
|
||||
|
||||
// Integrate positions.
|
||||
for (int i = 0; i < BodyCount; ++i)
|
||||
{
|
||||
Vector2 c = _positions[i].c;
|
||||
float a = _positions[i].a;
|
||||
Vector2 v = _velocities[i].v;
|
||||
float w = _velocities[i].w;
|
||||
|
||||
// Check for large velocities
|
||||
Vector2 translation = h * v;
|
||||
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
|
||||
{
|
||||
float ratio = Settings.MaxTranslation / translation.Length();
|
||||
v *= ratio;
|
||||
}
|
||||
|
||||
float rotation = h * w;
|
||||
if (rotation * rotation > Settings.MaxRotationSquared)
|
||||
{
|
||||
float ratio = Settings.MaxRotation / Math.Abs(rotation);
|
||||
w *= ratio;
|
||||
}
|
||||
|
||||
// Integrate
|
||||
c += h * v;
|
||||
a += h * w;
|
||||
|
||||
_positions[i].c = c;
|
||||
_positions[i].a = a;
|
||||
_velocities[i].v = v;
|
||||
_velocities[i].w = w;
|
||||
|
||||
// Sync bodies
|
||||
Body body = Bodies[i];
|
||||
body._sweep.C = c;
|
||||
body._sweep.A = a;
|
||||
body._linearVelocity = v;
|
||||
body._angularVelocity = w;
|
||||
body.SynchronizeTransform();
|
||||
}
|
||||
|
||||
Report(_contactSolver._velocityConstraints);
|
||||
}
|
||||
|
||||
public void Add(Body body)
|
||||
{
|
||||
Debug.Assert(BodyCount < BodyCapacity);
|
||||
body.IslandIndex = BodyCount;
|
||||
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(ContactVelocityConstraint[] constraints)
|
||||
{
|
||||
if (_contactManager == null)
|
||||
return;
|
||||
|
||||
for (int i = 0; i < ContactCount; ++i)
|
||||
{
|
||||
Contact c = _contacts[i];
|
||||
|
||||
//FPE optimization: We don't store the impulses and send it to the delegate. We just send the whole contact.
|
||||
//FPE feature: added after collision
|
||||
if (c.FixtureA.AfterCollision != null)
|
||||
c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c, constraints[i]);
|
||||
|
||||
if (c.FixtureB.AfterCollision != null)
|
||||
c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c, constraints[i]);
|
||||
|
||||
if (_contactManager.PostSolve != null)
|
||||
{
|
||||
_contactManager.PostSolve(c, constraints[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*/
|
||||
|
||||
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
|
||||
{
|
||||
private float _bias;
|
||||
private float _jointError;
|
||||
private float _massFactor;
|
||||
private float _targetAngle;
|
||||
|
||||
internal AngleJoint()
|
||||
{
|
||||
JointType = JointType.Angle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor for AngleJoint
|
||||
/// </summary>
|
||||
/// <param name="bodyA">The first body</param>
|
||||
/// <param name="bodyB">The second body</param>
|
||||
public AngleJoint(Body bodyA, Body bodyB)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Angle;
|
||||
BiasFactor = .2f;
|
||||
MaxImpulse = float.MaxValue;
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.Position; }
|
||||
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorB
|
||||
{
|
||||
get { return BodyB.Position; }
|
||||
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The desired angle between BodyA and BodyB
|
||||
/// </summary>
|
||||
public float TargetAngle
|
||||
{
|
||||
get { return _targetAngle; }
|
||||
set
|
||||
{
|
||||
if (value != _targetAngle)
|
||||
{
|
||||
_targetAngle = value;
|
||||
WakeBodies();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the bias factor.
|
||||
/// Defaults to 0.2
|
||||
/// </summary>
|
||||
public float BiasFactor { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the maximum impulse
|
||||
/// Defaults to float.MaxValue
|
||||
/// </summary>
|
||||
public float MaxImpulse { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the softness of the joint
|
||||
/// Defaults to 0
|
||||
/// </summary>
|
||||
public float Softness { get; set; }
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
//TODO
|
||||
//return _inv_dt * _impulse;
|
||||
return Vector2.Zero;
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
int indexA = BodyA.IslandIndex;
|
||||
int indexB = BodyB.IslandIndex;
|
||||
|
||||
float aW = data.positions[indexA].a;
|
||||
float bW = data.positions[indexB].a;
|
||||
|
||||
_jointError = (bW - aW - TargetAngle);
|
||||
_bias = -BiasFactor * data.step.inv_dt * _jointError;
|
||||
_massFactor = (1 - Softness) / (BodyA._invI + BodyB._invI);
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
int indexA = BodyA.IslandIndex;
|
||||
int indexB = BodyB.IslandIndex;
|
||||
|
||||
float p = (_bias - data.velocities[indexB].w + data.velocities[indexA].w) * _massFactor;
|
||||
|
||||
data.velocities[indexA].w -= BodyA._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
|
||||
data.velocities[indexB].w += BodyB._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
//no position solving for this joint
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,331 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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
|
||||
{
|
||||
// Solver shared
|
||||
private float _bias;
|
||||
private float _gamma;
|
||||
private float _impulse;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _u;
|
||||
private Vector2 _rA;
|
||||
private Vector2 _rB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
private float _mass;
|
||||
|
||||
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="anchorA">The first body anchor</param>
|
||||
/// <param name="anchorB">The second body anchor</param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public DistanceJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Distance;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
{
|
||||
LocalAnchorA = bodyA.GetLocalPoint(ref anchorA);
|
||||
LocalAnchorB = bodyB.GetLocalPoint(ref anchorB);
|
||||
Length = (anchorB - anchorA).Length();
|
||||
}
|
||||
else
|
||||
{
|
||||
LocalAnchorA = anchorA;
|
||||
LocalAnchorB = anchorB;
|
||||
Length = (BodyB.GetWorldPoint(ref anchorB) - BodyA.GetWorldPoint(ref anchorA)).Length();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point relative to bodyA's origin.
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point relative to bodyB's origin.
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorB { get; set; }
|
||||
|
||||
public override sealed Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
||||
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
|
||||
}
|
||||
|
||||
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."); }
|
||||
}
|
||||
|
||||
/// <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. A value of 0
|
||||
/// disables softness.
|
||||
/// </summary>
|
||||
public float Frequency { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The damping ratio. 0 = no damping, 1 = critical damping.
|
||||
/// </summary>
|
||||
public float DampingRatio { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the reaction force given the inverse time step. Unit is N.
|
||||
/// </summary>
|
||||
/// <param name="invDt"></param>
|
||||
/// <returns></returns>
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
Vector2 F = (invDt * _impulse) * _u;
|
||||
return F;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the reaction torque given the inverse time step.
|
||||
/// Unit is N*m. This is always zero for a distance joint.
|
||||
/// </summary>
|
||||
/// <param name="invDt"></param>
|
||||
/// <returns></returns>
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
_u = cB + _rB - cA - _rA;
|
||||
|
||||
// Handle singularity.
|
||||
float length = _u.Length();
|
||||
if (length > Settings.LinearSlop)
|
||||
{
|
||||
_u *= 1.0f / length;
|
||||
}
|
||||
else
|
||||
{
|
||||
_u = Vector2.Zero;
|
||||
}
|
||||
|
||||
float crAu = MathUtils.Cross(_rA, _u);
|
||||
float crBu = MathUtils.Cross(_rB, _u);
|
||||
float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;
|
||||
|
||||
// Compute the effective mass matrix.
|
||||
_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
|
||||
float h = data.step.dt;
|
||||
_gamma = h * (d + h * k);
|
||||
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
|
||||
_bias = C * h * k * _gamma;
|
||||
|
||||
invMass += _gamma;
|
||||
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
_gamma = 0.0f;
|
||||
_bias = 0.0f;
|
||||
}
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
// Scale the impulse to support a variable time step.
|
||||
_impulse *= data.step.dtRatio;
|
||||
|
||||
Vector2 P = _impulse * _u;
|
||||
vA -= _invMassA * P;
|
||||
wA -= _invIA * MathUtils.Cross(_rA, P);
|
||||
vB += _invMassB * P;
|
||||
wB += _invIB * MathUtils.Cross(_rB, P);
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
// Cdot = dot(u, v + cross(w, r))
|
||||
Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
|
||||
Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
|
||||
float Cdot = Vector2.Dot(_u, vpB - vpA);
|
||||
|
||||
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
|
||||
_impulse += impulse;
|
||||
|
||||
Vector2 P = impulse * _u;
|
||||
vA -= _invMassA * P;
|
||||
wA -= _invIA * MathUtils.Cross(_rA, P);
|
||||
vB += _invMassB * P;
|
||||
wB += _invIB * MathUtils.Cross(_rB, P);
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
if (Frequency > 0.0f)
|
||||
{
|
||||
// There is no position correction for soft distance constraints.
|
||||
return true;
|
||||
}
|
||||
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
Vector2 u = cB + rB - cA - rA;
|
||||
|
||||
float length = u.Length(); u.Normalize();
|
||||
float C = length - Length;
|
||||
C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
|
||||
|
||||
float impulse = -_mass * C;
|
||||
Vector2 P = impulse * u;
|
||||
|
||||
cA -= _invMassA * P;
|
||||
aA -= _invIA * MathUtils.Cross(rA, P);
|
||||
cB += _invMassB * P;
|
||||
aB += _invIB * MathUtils.Cross(rB, P);
|
||||
|
||||
data.positions[_indexA].c = cA;
|
||||
data.positions[_indexA].a = aA;
|
||||
data.positions[_indexB].c = cB;
|
||||
data.positions[_indexB].a = aB;
|
||||
|
||||
return Math.Abs(C) < Settings.LinearSlop;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,261 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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
|
||||
{
|
||||
// p = attached point, m = mouse point
|
||||
// C = p - m
|
||||
// Cdot = v
|
||||
// = v + cross(w, r)
|
||||
// J = [I r_skew]
|
||||
// Identity used:
|
||||
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
||||
|
||||
/// <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
|
||||
{
|
||||
private Vector2 _worldAnchor;
|
||||
private float _frequency;
|
||||
private float _dampingRatio;
|
||||
private float _beta;
|
||||
|
||||
// Solver shared
|
||||
private Vector2 _impulse;
|
||||
private float _maxForce;
|
||||
private float _gamma;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private Vector2 _rA;
|
||||
private Vector2 _localCenterA;
|
||||
private float _invMassA;
|
||||
private float _invIA;
|
||||
private Mat22 _mass;
|
||||
private Vector2 _C;
|
||||
|
||||
/// <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;
|
||||
MaxForce = 1000 * body.Mass;
|
||||
|
||||
Debug.Assert(worldAnchor.IsValid());
|
||||
|
||||
_worldAnchor = worldAnchor;
|
||||
LocalAnchorA = MathUtils.MulT(BodyA._xf, worldAnchor);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
||||
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorB
|
||||
{
|
||||
get { return _worldAnchor; }
|
||||
set
|
||||
{
|
||||
WakeBodies();
|
||||
_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 { return _maxForce; }
|
||||
set
|
||||
{
|
||||
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
|
||||
_maxForce = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The response speed.
|
||||
/// </summary>
|
||||
public float Frequency
|
||||
{
|
||||
get { return _frequency; }
|
||||
set
|
||||
{
|
||||
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
|
||||
_frequency = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The damping ratio. 0 = no damping, 1 = critical damping.
|
||||
/// </summary>
|
||||
public float DampingRatio
|
||||
{
|
||||
get { return _dampingRatio; }
|
||||
set
|
||||
{
|
||||
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
|
||||
_dampingRatio = value;
|
||||
}
|
||||
}
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
return invDt * _impulse;
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return invDt * 0.0f;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
Rot qA = new Rot(aA);
|
||||
|
||||
float mass = BodyA.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.
|
||||
float h = data.step.dt;
|
||||
Debug.Assert(d + h * k > Settings.Epsilon);
|
||||
_gamma = h * (d + h * k);
|
||||
if (_gamma != 0.0f)
|
||||
{
|
||||
_gamma = 1.0f / _gamma;
|
||||
}
|
||||
|
||||
_beta = h * k * _gamma;
|
||||
|
||||
// Compute the effective mass matrix.
|
||||
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
// 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]
|
||||
Mat22 K = new Mat22();
|
||||
K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma;
|
||||
K.ex.Y = -_invIA * _rA.X * _rA.Y;
|
||||
K.ey.X = K.ex.Y;
|
||||
K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma;
|
||||
|
||||
_mass = K.Inverse;
|
||||
|
||||
_C = cA + _rA - _worldAnchor;
|
||||
_C *= _beta;
|
||||
|
||||
// Cheat with some damping
|
||||
wA *= 0.98f;
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
_impulse *= data.step.dtRatio;
|
||||
vA += _invMassA * _impulse;
|
||||
wA += _invIA * MathUtils.Cross(_rA, _impulse);
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = Vector2.Zero;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
// Cdot = v + cross(w, r)
|
||||
Vector2 Cdot = vA + MathUtils.Cross(wA, _rA);
|
||||
Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse));
|
||||
|
||||
Vector2 oldImpulse = _impulse;
|
||||
_impulse += impulse;
|
||||
float maxImpulse = data.step.dt * MaxForce;
|
||||
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
|
||||
{
|
||||
_impulse *= maxImpulse / _impulse.Length();
|
||||
}
|
||||
impulse = _impulse - oldImpulse;
|
||||
|
||||
vA += _invMassA * impulse;
|
||||
wA += _invIA * MathUtils.Cross(_rA, impulse);
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,272 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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.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
|
||||
{
|
||||
// Solver shared
|
||||
private Vector2 _linearImpulse;
|
||||
private float _angularImpulse;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _rA;
|
||||
private Vector2 _rB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
private float _angularMass;
|
||||
private Mat22 _linearMass;
|
||||
|
||||
internal FrictionJoint()
|
||||
{
|
||||
JointType = JointType.Friction;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor for FrictionJoint.
|
||||
/// </summary>
|
||||
/// <param name="bodyA"></param>
|
||||
/// <param name="bodyB"></param>
|
||||
/// <param name="anchor"></param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public FrictionJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Friction;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
{
|
||||
LocalAnchorA = BodyA.GetLocalPoint(anchor);
|
||||
LocalAnchorB = BodyB.GetLocalPoint(anchor);
|
||||
}
|
||||
else
|
||||
{
|
||||
LocalAnchorA = anchor;
|
||||
LocalAnchorB = anchor;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyB
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorB { get; set; }
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
||||
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorB
|
||||
{
|
||||
get { return BodyB.GetWorldPoint(LocalAnchorB); }
|
||||
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
/// <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 invDt)
|
||||
{
|
||||
return invDt * _linearImpulse;
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return invDt * _angularImpulse;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
// Compute the effective mass matrix.
|
||||
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
|
||||
// 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 = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
Mat22 K = new Mat22();
|
||||
K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
|
||||
K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
|
||||
K.ey.X = K.ex.Y;
|
||||
K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;
|
||||
|
||||
_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 *= data.step.dtRatio;
|
||||
_angularImpulse *= data.step.dtRatio;
|
||||
|
||||
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
|
||||
vA -= mA * P;
|
||||
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
|
||||
vB += mB * P;
|
||||
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
|
||||
}
|
||||
else
|
||||
{
|
||||
_linearImpulse = Vector2.Zero;
|
||||
_angularImpulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
float h = data.step.dt;
|
||||
|
||||
// Solve angular friction
|
||||
{
|
||||
float Cdot = wB - wA;
|
||||
float impulse = -_angularMass * Cdot;
|
||||
|
||||
float oldImpulse = _angularImpulse;
|
||||
float maxImpulse = h * 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.Mul(ref _linearMass, Cdot);
|
||||
Vector2 oldImpulse = _linearImpulse;
|
||||
_linearImpulse += impulse;
|
||||
|
||||
float maxImpulse = h * 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);
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,478 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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
|
||||
{
|
||||
// Gear Joint:
|
||||
// C0 = (coordinate1 + ratio * coordinate2)_initial
|
||||
// C = (coordinate1 + ratio * coordinate2) - C0 = 0
|
||||
// J = [J1 ratio * J2]
|
||||
// K = J * invM * JT
|
||||
// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
|
||||
//
|
||||
// Revolute:
|
||||
// coordinate = rotation
|
||||
// Cdot = angularVelocity
|
||||
// J = [0 0 1]
|
||||
// K = J * invM * JT = invI
|
||||
//
|
||||
// Prismatic:
|
||||
// coordinate = dot(p - pg, ug)
|
||||
// Cdot = dot(v + cross(w, r), ug)
|
||||
// J = [ug cross(r, ug)]
|
||||
// K = J * invM * JT = invMass + invI * cross(r, ug)^2
|
||||
|
||||
/// <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:
|
||||
/// <![CDATA[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: You have to manually destroy the gear joint if jointA or jointB is destroyed.
|
||||
/// </summary>
|
||||
public class GearJoint : Joint
|
||||
{
|
||||
private JointType _typeA;
|
||||
private JointType _typeB;
|
||||
|
||||
private Body _bodyA;
|
||||
private Body _bodyB;
|
||||
private Body _bodyC;
|
||||
private Body _bodyD;
|
||||
|
||||
// Solver shared
|
||||
private Vector2 _localAnchorA;
|
||||
private Vector2 _localAnchorB;
|
||||
private Vector2 _localAnchorC;
|
||||
private Vector2 _localAnchorD;
|
||||
|
||||
private Vector2 _localAxisC;
|
||||
private Vector2 _localAxisD;
|
||||
|
||||
private float _referenceAngleA;
|
||||
private float _referenceAngleB;
|
||||
|
||||
private float _constant;
|
||||
private float _ratio;
|
||||
|
||||
private float _impulse;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA, _indexB, _indexC, _indexD;
|
||||
private Vector2 _lcA, _lcB, _lcC, _lcD;
|
||||
private float _mA, _mB, _mC, _mD;
|
||||
private float _iA, _iB, _iC, _iD;
|
||||
private Vector2 _JvAC, _JvBD;
|
||||
private float _JwA, _JwB, _JwC, _JwD;
|
||||
private float _mass;
|
||||
|
||||
/// <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>
|
||||
/// <param name="bodyA">The first body</param>
|
||||
/// <param name="bodyB">The second body</param>
|
||||
public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio = 1f)
|
||||
{
|
||||
JointType = JointType.Gear;
|
||||
BodyA = bodyA;
|
||||
BodyB = bodyB;
|
||||
JointA = jointA;
|
||||
JointB = jointB;
|
||||
Ratio = ratio;
|
||||
|
||||
_typeA = jointA.JointType;
|
||||
_typeB = jointB.JointType;
|
||||
|
||||
Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic);
|
||||
Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic);
|
||||
|
||||
float coordinateA, coordinateB;
|
||||
|
||||
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
|
||||
|
||||
_bodyC = JointA.BodyA;
|
||||
_bodyA = JointA.BodyB;
|
||||
|
||||
// Get geometry of joint1
|
||||
Transform xfA = _bodyA._xf;
|
||||
float aA = _bodyA._sweep.A;
|
||||
Transform xfC = _bodyC._xf;
|
||||
float aC = _bodyC._sweep.A;
|
||||
|
||||
if (_typeA == JointType.Revolute)
|
||||
{
|
||||
RevoluteJoint revolute = (RevoluteJoint)jointA;
|
||||
_localAnchorC = revolute.LocalAnchorA;
|
||||
_localAnchorA = revolute.LocalAnchorB;
|
||||
_referenceAngleA = revolute.ReferenceAngle;
|
||||
_localAxisC = Vector2.Zero;
|
||||
|
||||
coordinateA = aA - aC - _referenceAngleA;
|
||||
}
|
||||
else
|
||||
{
|
||||
PrismaticJoint prismatic = (PrismaticJoint)jointA;
|
||||
_localAnchorC = prismatic.LocalAnchorA;
|
||||
_localAnchorA = prismatic.LocalAnchorB;
|
||||
_referenceAngleA = prismatic.ReferenceAngle;
|
||||
_localAxisC = prismatic.LocalXAxis;
|
||||
|
||||
Vector2 pC = _localAnchorC;
|
||||
Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
|
||||
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
|
||||
}
|
||||
|
||||
_bodyD = JointB.BodyA;
|
||||
_bodyB = JointB.BodyB;
|
||||
|
||||
// Get geometry of joint2
|
||||
Transform xfB = _bodyB._xf;
|
||||
float aB = _bodyB._sweep.A;
|
||||
Transform xfD = _bodyD._xf;
|
||||
float aD = _bodyD._sweep.A;
|
||||
|
||||
if (_typeB == JointType.Revolute)
|
||||
{
|
||||
RevoluteJoint revolute = (RevoluteJoint)jointB;
|
||||
_localAnchorD = revolute.LocalAnchorA;
|
||||
_localAnchorB = revolute.LocalAnchorB;
|
||||
_referenceAngleB = revolute.ReferenceAngle;
|
||||
_localAxisD = Vector2.Zero;
|
||||
|
||||
coordinateB = aB - aD - _referenceAngleB;
|
||||
}
|
||||
else
|
||||
{
|
||||
PrismaticJoint prismatic = (PrismaticJoint)jointB;
|
||||
_localAnchorD = prismatic.LocalAnchorA;
|
||||
_localAnchorB = prismatic.LocalAnchorB;
|
||||
_referenceAngleB = prismatic.ReferenceAngle;
|
||||
_localAxisD = prismatic.LocalXAxis;
|
||||
|
||||
Vector2 pD = _localAnchorD;
|
||||
Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
|
||||
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
|
||||
}
|
||||
|
||||
_ratio = ratio;
|
||||
_constant = coordinateA + _ratio * coordinateB;
|
||||
_impulse = 0.0f;
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return _bodyA.GetWorldPoint(_localAnchorA); }
|
||||
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
|
||||
}
|
||||
|
||||
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 gear ratio.
|
||||
/// </summary>
|
||||
public float Ratio
|
||||
{
|
||||
get { return _ratio; }
|
||||
set
|
||||
{
|
||||
Debug.Assert(MathUtils.IsValid(value));
|
||||
_ratio = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The first revolute/prismatic joint attached to the gear joint.
|
||||
/// </summary>
|
||||
public Joint JointA { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// The second revolute/prismatic joint attached to the gear joint.
|
||||
/// </summary>
|
||||
public Joint JointB { get; private set; }
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
Vector2 P = _impulse * _JvAC;
|
||||
return invDt * P;
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
float L = _impulse * _JwA;
|
||||
return invDt * L;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = _bodyA.IslandIndex;
|
||||
_indexB = _bodyB.IslandIndex;
|
||||
_indexC = _bodyC.IslandIndex;
|
||||
_indexD = _bodyD.IslandIndex;
|
||||
_lcA = _bodyA._sweep.LocalCenter;
|
||||
_lcB = _bodyB._sweep.LocalCenter;
|
||||
_lcC = _bodyC._sweep.LocalCenter;
|
||||
_lcD = _bodyD._sweep.LocalCenter;
|
||||
_mA = _bodyA._invMass;
|
||||
_mB = _bodyB._invMass;
|
||||
_mC = _bodyC._invMass;
|
||||
_mD = _bodyD._invMass;
|
||||
_iA = _bodyA._invI;
|
||||
_iB = _bodyB._invI;
|
||||
_iC = _bodyC._invI;
|
||||
_iD = _bodyD._invI;
|
||||
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
float aC = data.positions[_indexC].a;
|
||||
Vector2 vC = data.velocities[_indexC].v;
|
||||
float wC = data.velocities[_indexC].w;
|
||||
|
||||
float aD = data.positions[_indexD].a;
|
||||
Vector2 vD = data.velocities[_indexD].v;
|
||||
float wD = data.velocities[_indexD].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
|
||||
|
||||
_mass = 0.0f;
|
||||
|
||||
if (_typeA == JointType.Revolute)
|
||||
{
|
||||
_JvAC = Vector2.Zero;
|
||||
_JwA = 1.0f;
|
||||
_JwC = 1.0f;
|
||||
_mass += _iA + _iC;
|
||||
}
|
||||
else
|
||||
{
|
||||
Vector2 u = MathUtils.Mul(qC, _localAxisC);
|
||||
Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
|
||||
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
|
||||
_JvAC = u;
|
||||
_JwC = MathUtils.Cross(rC, u);
|
||||
_JwA = MathUtils.Cross(rA, u);
|
||||
_mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA;
|
||||
}
|
||||
|
||||
if (_typeB == JointType.Revolute)
|
||||
{
|
||||
_JvBD = Vector2.Zero;
|
||||
_JwB = _ratio;
|
||||
_JwD = _ratio;
|
||||
_mass += _ratio * _ratio * (_iB + _iD);
|
||||
}
|
||||
else
|
||||
{
|
||||
Vector2 u = MathUtils.Mul(qD, _localAxisD);
|
||||
Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
|
||||
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
|
||||
_JvBD = _ratio * u;
|
||||
_JwD = _ratio * MathUtils.Cross(rD, u);
|
||||
_JwB = _ratio * MathUtils.Cross(rB, u);
|
||||
_mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB;
|
||||
}
|
||||
|
||||
// Compute effective mass.
|
||||
_mass = _mass > 0.0f ? 1.0f / _mass : 0.0f;
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
vA += (_mA * _impulse) * _JvAC;
|
||||
wA += _iA * _impulse * _JwA;
|
||||
vB += (_mB * _impulse) * _JvBD;
|
||||
wB += _iB * _impulse * _JwB;
|
||||
vC -= (_mC * _impulse) * _JvAC;
|
||||
wC -= _iC * _impulse * _JwC;
|
||||
vD -= (_mD * _impulse) * _JvBD;
|
||||
wD -= _iD * _impulse * _JwD;
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
data.velocities[_indexC].v = vC;
|
||||
data.velocities[_indexC].w = wC;
|
||||
data.velocities[_indexD].v = vD;
|
||||
data.velocities[_indexD].w = wD;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
Vector2 vC = data.velocities[_indexC].v;
|
||||
float wC = data.velocities[_indexC].w;
|
||||
Vector2 vD = data.velocities[_indexD].v;
|
||||
float wD = data.velocities[_indexD].w;
|
||||
|
||||
float Cdot = Vector2.Dot(_JvAC, vA - vC) + Vector2.Dot(_JvBD, vB - vD);
|
||||
Cdot += (_JwA * wA - _JwC * wC) + (_JwB * wB - _JwD * wD);
|
||||
|
||||
float impulse = -_mass * Cdot;
|
||||
_impulse += impulse;
|
||||
|
||||
vA += (_mA * impulse) * _JvAC;
|
||||
wA += _iA * impulse * _JwA;
|
||||
vB += (_mB * impulse) * _JvBD;
|
||||
wB += _iB * impulse * _JwB;
|
||||
vC -= (_mC * impulse) * _JvAC;
|
||||
wC -= _iC * impulse * _JwC;
|
||||
vD -= (_mD * impulse) * _JvBD;
|
||||
wD -= _iD * impulse * _JwD;
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
data.velocities[_indexC].v = vC;
|
||||
data.velocities[_indexC].w = wC;
|
||||
data.velocities[_indexD].v = vD;
|
||||
data.velocities[_indexD].w = wD;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 cC = data.positions[_indexC].c;
|
||||
float aC = data.positions[_indexC].a;
|
||||
Vector2 cD = data.positions[_indexD].c;
|
||||
float aD = data.positions[_indexD].a;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
|
||||
|
||||
const float linearError = 0.0f;
|
||||
|
||||
float coordinateA, coordinateB;
|
||||
|
||||
Vector2 JvAC, JvBD;
|
||||
float JwA, JwB, JwC, JwD;
|
||||
float mass = 0.0f;
|
||||
|
||||
if (_typeA == JointType.Revolute)
|
||||
{
|
||||
JvAC = Vector2.Zero;
|
||||
JwA = 1.0f;
|
||||
JwC = 1.0f;
|
||||
mass += _iA + _iC;
|
||||
|
||||
coordinateA = aA - aC - _referenceAngleA;
|
||||
}
|
||||
else
|
||||
{
|
||||
Vector2 u = MathUtils.Mul(qC, _localAxisC);
|
||||
Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
|
||||
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
|
||||
JvAC = u;
|
||||
JwC = MathUtils.Cross(rC, u);
|
||||
JwA = MathUtils.Cross(rA, u);
|
||||
mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA;
|
||||
|
||||
Vector2 pC = _localAnchorC - _lcC;
|
||||
Vector2 pA = MathUtils.MulT(qC, rA + (cA - cC));
|
||||
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
|
||||
}
|
||||
|
||||
if (_typeB == JointType.Revolute)
|
||||
{
|
||||
JvBD = Vector2.Zero;
|
||||
JwB = _ratio;
|
||||
JwD = _ratio;
|
||||
mass += _ratio * _ratio * (_iB + _iD);
|
||||
|
||||
coordinateB = aB - aD - _referenceAngleB;
|
||||
}
|
||||
else
|
||||
{
|
||||
Vector2 u = MathUtils.Mul(qD, _localAxisD);
|
||||
Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
|
||||
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
|
||||
JvBD = _ratio * u;
|
||||
JwD = _ratio * MathUtils.Cross(rD, u);
|
||||
JwB = _ratio * MathUtils.Cross(rB, u);
|
||||
mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB;
|
||||
|
||||
Vector2 pD = _localAnchorD - _lcD;
|
||||
Vector2 pB = MathUtils.MulT(qD, rB + (cB - cD));
|
||||
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
|
||||
}
|
||||
|
||||
float C = (coordinateA + _ratio * coordinateB) - _constant;
|
||||
|
||||
float impulse = 0.0f;
|
||||
if (mass > 0.0f)
|
||||
{
|
||||
impulse = -C / mass;
|
||||
}
|
||||
|
||||
cA += _mA * impulse * JvAC;
|
||||
aA += _iA * impulse * JwA;
|
||||
cB += _mB * impulse * JvBD;
|
||||
aB += _iB * impulse * JwB;
|
||||
cC -= _mC * impulse * JvAC;
|
||||
aC -= _iC * impulse * JwC;
|
||||
cD -= _mD * impulse * JvBD;
|
||||
aD -= _iD * impulse * JwD;
|
||||
|
||||
data.positions[_indexA].c = cA;
|
||||
data.positions[_indexA].a = aA;
|
||||
data.positions[_indexB].c = cB;
|
||||
data.positions[_indexB].a = aB;
|
||||
data.positions[_indexC].c = cC;
|
||||
data.positions[_indexC].a = aC;
|
||||
data.positions[_indexD].c = cD;
|
||||
data.positions[_indexD].a = aD;
|
||||
|
||||
// TODO_ERIN not implemented
|
||||
return linearError < Settings.LinearSlop;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,253 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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
|
||||
{
|
||||
Unknown,
|
||||
Revolute,
|
||||
Prismatic,
|
||||
Distance,
|
||||
Pulley,
|
||||
//Mouse, <- We have fixed mouse
|
||||
Gear,
|
||||
Wheel,
|
||||
Weld,
|
||||
Friction,
|
||||
Rope,
|
||||
Motor,
|
||||
|
||||
//FPE note: From here on and down, it is only FPE joints
|
||||
Angle,
|
||||
FixedMouse,
|
||||
FixedRevolute,
|
||||
FixedDistance,
|
||||
FixedLine,
|
||||
FixedPrismatic,
|
||||
FixedAngle,
|
||||
FixedFriction,
|
||||
}
|
||||
|
||||
public enum LimitState
|
||||
{
|
||||
Inactive,
|
||||
AtLower,
|
||||
AtUpper,
|
||||
Equal,
|
||||
}
|
||||
|
||||
/// <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
|
||||
{
|
||||
private float _breakpoint;
|
||||
private double _breakpointSquared;
|
||||
|
||||
/// <summary>
|
||||
/// Indicate if this join is enabled or not. Disabling a joint
|
||||
/// means it is still in the simulation, but inactive.
|
||||
/// </summary>
|
||||
public bool Enabled = true;
|
||||
|
||||
internal JointEdge EdgeA = new JointEdge();
|
||||
internal JointEdge EdgeB = new JointEdge();
|
||||
internal bool IslandFlag;
|
||||
|
||||
protected Joint()
|
||||
{
|
||||
Breakpoint = float.MaxValue;
|
||||
|
||||
//Connected bodies should not collide by default
|
||||
CollideConnected = false;
|
||||
}
|
||||
|
||||
protected Joint(Body bodyA, Body bodyB) : this()
|
||||
{
|
||||
//Can't connect a joint to the same body twice.
|
||||
Debug.Assert(bodyA != bodyB);
|
||||
|
||||
BodyA = bodyA;
|
||||
BodyB = bodyB;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor for fixed joint
|
||||
/// </summary>
|
||||
protected Joint(Body body) : this()
|
||||
{
|
||||
BodyA = body;
|
||||
}
|
||||
|
||||
/// <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>
|
||||
public Body BodyA { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the second body attached to this joint.
|
||||
/// </summary>
|
||||
public Body BodyB { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the anchor point on bodyA in world coordinates.
|
||||
/// On some joints, this value indicate the anchor point within the world.
|
||||
/// </summary>
|
||||
public abstract Vector2 WorldAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the anchor point on bodyB in world coordinates.
|
||||
/// On some joints, this value indicate the anchor point within the world.
|
||||
/// </summary>
|
||||
public abstract Vector2 WorldAnchorB { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Set the user data pointer.
|
||||
/// </summary>
|
||||
/// <value>The data.</value>
|
||||
public object UserData { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Set this flag to true if the attached bodies should collide.
|
||||
/// </summary>
|
||||
public bool CollideConnected { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks.
|
||||
/// The default value is float.MaxValue, which means it never breaks.
|
||||
/// </summary>
|
||||
public float Breakpoint
|
||||
{
|
||||
get { return _breakpoint; }
|
||||
set
|
||||
{
|
||||
_breakpoint = value;
|
||||
_breakpointSquared = _breakpoint * _breakpoint;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fires when the joint is broken.
|
||||
/// </summary>
|
||||
public event Action<Joint, float> Broke;
|
||||
|
||||
/// <summary>
|
||||
/// Get the reaction force on body at the joint anchor in Newtons.
|
||||
/// </summary>
|
||||
/// <param name="invDt">The inverse delta time.</param>
|
||||
public abstract Vector2 GetReactionForce(float invDt);
|
||||
|
||||
/// <summary>
|
||||
/// Get the reaction torque on the body at the joint anchor in N*m.
|
||||
/// </summary>
|
||||
/// <param name="invDt">The inverse delta time.</param>
|
||||
public abstract float GetReactionTorque(float invDt);
|
||||
|
||||
protected void WakeBodies()
|
||||
{
|
||||
if (BodyA != null)
|
||||
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 SolverData data);
|
||||
|
||||
internal void Validate(float invDt)
|
||||
{
|
||||
if (!Enabled)
|
||||
return;
|
||||
|
||||
float jointErrorSquared = GetReactionForce(invDt).LengthSquared();
|
||||
|
||||
if (Math.Abs(jointErrorSquared) <= _breakpointSquared)
|
||||
return;
|
||||
|
||||
Enabled = false;
|
||||
|
||||
if (Broke != null)
|
||||
Broke(this, (float)Math.Sqrt(jointErrorSquared));
|
||||
}
|
||||
|
||||
internal abstract void SolveVelocityConstraints(ref SolverData data);
|
||||
|
||||
/// <summary>
|
||||
/// Solves the position constraints.
|
||||
/// </summary>
|
||||
/// <param name="data"></param>
|
||||
/// <returns>returns true if the position errors are within tolerance.</returns>
|
||||
internal abstract bool SolvePositionConstraints(ref SolverData data);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,320 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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 motor joint is used to control the relative motion
|
||||
/// between two bodies. A typical usage is to control the movement
|
||||
/// of a dynamic body with respect to the ground.
|
||||
/// </summary>
|
||||
public class MotorJoint : Joint
|
||||
{
|
||||
// Solver shared
|
||||
private Vector2 _linearOffset;
|
||||
private float _angularOffset;
|
||||
private Vector2 _linearImpulse;
|
||||
private float _angularImpulse;
|
||||
private float _maxForce;
|
||||
private float _maxTorque;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _rA;
|
||||
private Vector2 _rB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private Vector2 _linearError;
|
||||
private float _angularError;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
private Mat22 _linearMass;
|
||||
private float _angularMass;
|
||||
|
||||
internal MotorJoint()
|
||||
{
|
||||
JointType = JointType.Motor;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor for MotorJoint.
|
||||
/// </summary>
|
||||
/// <param name="bodyA">The first body</param>
|
||||
/// <param name="bodyB">The second body</param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public MotorJoint(Body bodyA, Body bodyB, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Motor;
|
||||
|
||||
Vector2 xB = BodyB.Position;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
_linearOffset = BodyA.GetLocalPoint(xB);
|
||||
else
|
||||
_linearOffset = xB;
|
||||
|
||||
//Defaults
|
||||
_angularOffset = 0.0f;
|
||||
_maxForce = 1.0f;
|
||||
_maxTorque = 1.0f;
|
||||
CorrectionFactor = 0.3f;
|
||||
|
||||
_angularOffset = BodyB.Rotation - BodyA.Rotation;
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.Position; }
|
||||
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorB
|
||||
{
|
||||
get { return BodyB.Position; }
|
||||
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The maximum amount of force that can be applied to BodyA
|
||||
/// </summary>
|
||||
public float MaxForce
|
||||
{
|
||||
set
|
||||
{
|
||||
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
|
||||
_maxForce = value;
|
||||
}
|
||||
get { return _maxForce; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The maximum amount of torque that can be applied to BodyA
|
||||
/// </summary>
|
||||
public float MaxTorque
|
||||
{
|
||||
set
|
||||
{
|
||||
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
|
||||
_maxTorque = value;
|
||||
}
|
||||
get { return _maxTorque; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The linear (translation) offset.
|
||||
/// </summary>
|
||||
public Vector2 LinearOffset
|
||||
{
|
||||
set
|
||||
{
|
||||
if (_linearOffset.X != value.X || _linearOffset.Y != value.Y)
|
||||
{
|
||||
WakeBodies();
|
||||
_linearOffset = value;
|
||||
}
|
||||
}
|
||||
get { return _linearOffset; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get or set the angular offset.
|
||||
/// </summary>
|
||||
public float AngularOffset
|
||||
{
|
||||
set
|
||||
{
|
||||
if (_angularOffset != value)
|
||||
{
|
||||
WakeBodies();
|
||||
_angularOffset = value;
|
||||
}
|
||||
}
|
||||
get { return _angularOffset; }
|
||||
}
|
||||
|
||||
//FPE note: Used for serialization.
|
||||
internal float CorrectionFactor { get; set; }
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
return invDt * _linearImpulse;
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return invDt * _angularImpulse;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA);
|
||||
Rot qB = new Rot(aB);
|
||||
|
||||
// Compute the effective mass matrix.
|
||||
_rA = MathUtils.Mul(qA, -_localCenterA);
|
||||
_rB = MathUtils.Mul(qB, -_localCenterB);
|
||||
|
||||
// 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 = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
Mat22 K = new Mat22();
|
||||
K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
|
||||
K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
|
||||
K.ey.X = K.ex.Y;
|
||||
K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;
|
||||
|
||||
_linearMass = K.Inverse;
|
||||
|
||||
_angularMass = iA + iB;
|
||||
if (_angularMass > 0.0f)
|
||||
{
|
||||
_angularMass = 1.0f / _angularMass;
|
||||
}
|
||||
|
||||
_linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset);
|
||||
_angularError = aB - aA - _angularOffset;
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
// Scale impulses to support a variable time step.
|
||||
_linearImpulse *= data.step.dtRatio;
|
||||
_angularImpulse *= data.step.dtRatio;
|
||||
|
||||
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
|
||||
vB += mB * P;
|
||||
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
|
||||
}
|
||||
else
|
||||
{
|
||||
_linearImpulse = Vector2.Zero;
|
||||
_angularImpulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
float h = data.step.dt;
|
||||
float inv_h = data.step.inv_dt;
|
||||
|
||||
// Solve angular friction
|
||||
{
|
||||
float Cdot = wB - wA + inv_h * CorrectionFactor * _angularError;
|
||||
float impulse = -_angularMass * Cdot;
|
||||
|
||||
float oldImpulse = _angularImpulse;
|
||||
float maxImpulse = h * _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) + inv_h * CorrectionFactor * _linearError;
|
||||
|
||||
Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot);
|
||||
Vector2 oldImpulse = _linearImpulse;
|
||||
_linearImpulse += impulse;
|
||||
|
||||
float maxImpulse = h * _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);
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,768 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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 bodyA. 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
|
||||
{
|
||||
private Vector2 _localYAxisA;
|
||||
private Vector3 _impulse;
|
||||
private float _lowerTranslation;
|
||||
private float _upperTranslation;
|
||||
private float _maxMotorForce;
|
||||
private float _motorSpeed;
|
||||
private bool _enableLimit;
|
||||
private bool _enableMotor;
|
||||
private LimitState _limitState;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
private Vector2 _axis, _perp;
|
||||
private float _s1, _s2;
|
||||
private float _a1, _a2;
|
||||
private Mat33 _K;
|
||||
private float _motorMass;
|
||||
private Vector2 _axis1;
|
||||
|
||||
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="anchorA">The first body anchor.</param>
|
||||
/// <param name="anchorB">The second body anchor.</param>
|
||||
/// <param name="axis">The axis.</param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 axis, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
Initialize(anchorA, anchorB, axis, useWorldCoordinates);
|
||||
}
|
||||
|
||||
public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
Initialize(anchor, anchor, axis, useWorldCoordinates);
|
||||
}
|
||||
|
||||
private void Initialize(Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis, bool useWorldCoordinates)
|
||||
{
|
||||
JointType = JointType.Prismatic;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
{
|
||||
LocalAnchorA = BodyA.GetLocalPoint(localAnchorA);
|
||||
LocalAnchorB = BodyB.GetLocalPoint(localAnchorB);
|
||||
}
|
||||
else
|
||||
{
|
||||
LocalAnchorA = localAnchorA;
|
||||
LocalAnchorB = localAnchorB;
|
||||
}
|
||||
|
||||
Axis = axis; //FPE only: store the orignal value for use in Serialization
|
||||
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
|
||||
|
||||
_limitState = LimitState.Inactive;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyB
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorB { get; set; }
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
||||
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorB
|
||||
{
|
||||
get { return BodyB.GetWorldPoint(LocalAnchorB); }
|
||||
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
/// <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(LocalXAxis);
|
||||
|
||||
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.Mul(ref xf1.q, LocalAnchorA - BodyA.LocalCenter);
|
||||
Vector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - BodyB.LocalCenter);
|
||||
Vector2 p1 = BodyA._sweep.C + r1;
|
||||
Vector2 p2 = BodyB._sweep.C + r2;
|
||||
Vector2 d = p2 - p1;
|
||||
Vector2 axis = BodyA.GetWorldVector(LocalXAxis);
|
||||
|
||||
Vector2 v1 = BodyA._linearVelocity;
|
||||
Vector2 v2 = BodyB._linearVelocity;
|
||||
float w1 = BodyA._angularVelocity;
|
||||
float w2 = BodyB._angularVelocity;
|
||||
|
||||
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");
|
||||
|
||||
if (value != _enableLimit)
|
||||
{
|
||||
WakeBodies();
|
||||
_enableLimit = value;
|
||||
_impulse.Z = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the lower joint limit, usually in meters.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public float LowerLimit
|
||||
{
|
||||
get { return _lowerTranslation; }
|
||||
set
|
||||
{
|
||||
if (value != _lowerTranslation)
|
||||
{
|
||||
WakeBodies();
|
||||
_lowerTranslation = value;
|
||||
_impulse.Z = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the upper joint limit, usually in meters.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public float UpperLimit
|
||||
{
|
||||
get { return _upperTranslation; }
|
||||
set
|
||||
{
|
||||
if (value != _upperTranslation)
|
||||
{
|
||||
WakeBodies();
|
||||
_upperTranslation = value;
|
||||
_impulse.Z = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Set the joint limits, usually in meters.
|
||||
/// </summary>
|
||||
/// <param name="lower">The lower limit</param>
|
||||
/// <param name="upper">The upper limit</param>
|
||||
public void SetLimits(float lower, float upper)
|
||||
{
|
||||
if (upper != _upperTranslation || lower != _lowerTranslation)
|
||||
{
|
||||
WakeBodies();
|
||||
_upperTranslation = upper;
|
||||
_lowerTranslation = lower;
|
||||
_impulse.Z = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/// <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 impulse, usually in N.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public float MotorImpulse { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the motor force.
|
||||
/// </summary>
|
||||
/// <param name="invDt">The inverse delta time</param>
|
||||
public float GetMotorForce(float invDt)
|
||||
{
|
||||
return invDt * MotorImpulse;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The axis at which the joint moves.
|
||||
/// </summary>
|
||||
public Vector2 Axis
|
||||
{
|
||||
get { return _axis1; }
|
||||
set
|
||||
{
|
||||
_axis1 = value;
|
||||
LocalXAxis = BodyA.GetLocalVector(_axis1);
|
||||
LocalXAxis.Normalize();
|
||||
_localYAxisA = MathUtils.Cross(1.0f, LocalXAxis);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The axis in local coordinates relative to BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalXAxis { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// The reference angle.
|
||||
/// </summary>
|
||||
public float ReferenceAngle { get; set; }
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
return invDt * (_impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis);
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return invDt * _impulse.Y;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
// Compute the effective masses.
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
Vector2 d = (cB - cA) + rB - rA;
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
// Compute motor Jacobian and effective mass.
|
||||
{
|
||||
_axis = MathUtils.Mul(qA, LocalXAxis);
|
||||
_a1 = MathUtils.Cross(d + rA, _axis);
|
||||
_a2 = MathUtils.Cross(rB, _axis);
|
||||
|
||||
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
|
||||
if (_motorMass > 0.0f)
|
||||
{
|
||||
_motorMass = 1.0f / _motorMass;
|
||||
}
|
||||
}
|
||||
|
||||
// Prismatic constraint.
|
||||
{
|
||||
_perp = MathUtils.Mul(qA, _localYAxisA);
|
||||
|
||||
_s1 = MathUtils.Cross(d + rA, _perp);
|
||||
_s2 = MathUtils.Cross(rB, _perp);
|
||||
|
||||
float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
|
||||
float k12 = iA * _s1 + iB * _s2;
|
||||
float k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
|
||||
float k22 = iA + iB;
|
||||
if (k22 == 0.0f)
|
||||
{
|
||||
// For bodies with fixed rotation.
|
||||
k22 = 1.0f;
|
||||
}
|
||||
float k23 = iA * _a1 + iB * _a2;
|
||||
float k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
|
||||
|
||||
_K.ex = new Vector3(k11, k12, k13);
|
||||
_K.ey = new Vector3(k12, k22, k23);
|
||||
_K.ez = 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;
|
||||
_impulse.Z = 0.0f;
|
||||
}
|
||||
|
||||
if (_enableMotor == false)
|
||||
{
|
||||
MotorImpulse = 0.0f;
|
||||
}
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
// Account for variable time step.
|
||||
_impulse *= data.step.dtRatio;
|
||||
MotorImpulse *= data.step.dtRatio;
|
||||
|
||||
Vector2 P = _impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis;
|
||||
float LA = _impulse.X * _s1 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a1;
|
||||
float LB = _impulse.X * _s2 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a2;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * LA;
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * LB;
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = Vector3.Zero;
|
||||
MotorImpulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
// Solve linear motor constraint.
|
||||
if (_enableMotor && _limitState != LimitState.Equal)
|
||||
{
|
||||
float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
|
||||
float impulse = _motorMass * (_motorSpeed - Cdot);
|
||||
float oldImpulse = MotorImpulse;
|
||||
float maxImpulse = data.step.dt * _maxMotorForce;
|
||||
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
|
||||
impulse = MotorImpulse - oldImpulse;
|
||||
|
||||
Vector2 P = impulse * _axis;
|
||||
float LA = impulse * _a1;
|
||||
float LB = impulse * _a2;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * LA;
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * LB;
|
||||
}
|
||||
|
||||
Vector2 Cdot1 = new Vector2();
|
||||
Cdot1.X = Vector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA;
|
||||
Cdot1.Y = wB - wA;
|
||||
|
||||
if (_enableLimit && _limitState != LimitState.Inactive)
|
||||
{
|
||||
// Solve prismatic and limit constraint in block form.
|
||||
float Cdot2;
|
||||
Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
|
||||
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.ez.X, _K.ez.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 LA = df.X * _s1 + df.Y + df.Z * _a1;
|
||||
float LB = df.X * _s2 + df.Y + df.Z * _a2;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * LA;
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * LB;
|
||||
}
|
||||
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 LA = df.X * _s1 + df.Y;
|
||||
float LB = df.X * _s2 + df.Y;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * LA;
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * LB;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
// Compute fresh Jacobians
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
Vector2 d = cB + rB - cA - rA;
|
||||
|
||||
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
|
||||
float a1 = MathUtils.Cross(d + rA, axis);
|
||||
float a2 = MathUtils.Cross(rB, axis);
|
||||
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
|
||||
|
||||
float s1 = MathUtils.Cross(d + rA, perp);
|
||||
float s2 = MathUtils.Cross(rB, perp);
|
||||
|
||||
Vector3 impulse;
|
||||
Vector2 C1 = new Vector2();
|
||||
C1.X = Vector2.Dot(perp, d);
|
||||
C1.Y = aB - aA - ReferenceAngle;
|
||||
|
||||
float linearError = Math.Abs(C1.X);
|
||||
float angularError = Math.Abs(C1.Y);
|
||||
|
||||
bool active = false;
|
||||
float C2 = 0.0f;
|
||||
if (_enableLimit)
|
||||
{
|
||||
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.Max(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 = Math.Max(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 = Math.Max(linearError, translation - _upperTranslation);
|
||||
active = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (active)
|
||||
{
|
||||
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
|
||||
float k12 = iA * s1 + iB * s2;
|
||||
float k13 = iA * s1 * a1 + iB * s2 * a2;
|
||||
float k22 = iA + iB;
|
||||
if (k22 == 0.0f)
|
||||
{
|
||||
// For fixed rotation
|
||||
k22 = 1.0f;
|
||||
}
|
||||
float k23 = iA * a1 + iB * a2;
|
||||
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
|
||||
|
||||
Mat33 K = new Mat33();
|
||||
K.ex = new Vector3(k11, k12, k13);
|
||||
K.ey = new Vector3(k12, k22, k23);
|
||||
K.ez = new Vector3(k13, k23, k33);
|
||||
|
||||
Vector3 C = new Vector3();
|
||||
C.X = C1.X;
|
||||
C.Y = C1.Y;
|
||||
C.Z = C2;
|
||||
|
||||
impulse = K.Solve33(-C);
|
||||
}
|
||||
else
|
||||
{
|
||||
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
|
||||
float k12 = iA * s1 + iB * s2;
|
||||
float k22 = iA + iB;
|
||||
if (k22 == 0.0f)
|
||||
{
|
||||
k22 = 1.0f;
|
||||
}
|
||||
|
||||
Mat22 K = new Mat22();
|
||||
K.ex = new Vector2(k11, k12);
|
||||
K.ey = new Vector2(k12, k22);
|
||||
|
||||
Vector2 impulse1 = K.Solve(-C1);
|
||||
impulse = new Vector3();
|
||||
impulse.X = impulse1.X;
|
||||
impulse.Y = impulse1.Y;
|
||||
impulse.Z = 0.0f;
|
||||
}
|
||||
|
||||
Vector2 P = impulse.X * perp + impulse.Z * axis;
|
||||
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
|
||||
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
|
||||
|
||||
cA -= mA * P;
|
||||
aA -= iA * LA;
|
||||
cB += mB * P;
|
||||
aB += iB * LB;
|
||||
|
||||
data.positions[_indexA].c = cA;
|
||||
data.positions[_indexA].a = aA;
|
||||
data.positions[_indexB].c = cB;
|
||||
data.positions[_indexB].a = aB;
|
||||
|
||||
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,396 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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
|
||||
{
|
||||
// Pulley:
|
||||
// length1 = norm(p1 - s1)
|
||||
// length2 = norm(p2 - s2)
|
||||
// C0 = (length1 + ratio * length2)_initial
|
||||
// C = C0 - (length1 + ratio * length2)
|
||||
// u1 = (p1 - s1) / norm(p1 - s1)
|
||||
// u2 = (p2 - s2) / norm(p2 - s2)
|
||||
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
|
||||
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
|
||||
// K = J * invM * JT
|
||||
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
|
||||
|
||||
/// <summary>
|
||||
/// The pulley joint is connected to two bodies and two fixed world points.
|
||||
/// The pulley supports a ratio such that:
|
||||
/// <![CDATA[length1 + ratio * length2 <= constant]]>
|
||||
/// Yes, the force transmitted is scaled by the ratio.
|
||||
///
|
||||
/// Warning: the pulley joint can get a bit squirrelly by itself. They often
|
||||
/// work better when combined with prismatic joints. You should also cover the
|
||||
/// the anchor points with static shapes to prevent one side from going to zero length.
|
||||
/// </summary>
|
||||
public class PulleyJoint : Joint
|
||||
{
|
||||
// Solver shared
|
||||
private float _impulse;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _uA;
|
||||
private Vector2 _uB;
|
||||
private Vector2 _rA;
|
||||
private Vector2 _rB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
private float _mass;
|
||||
|
||||
internal PulleyJoint()
|
||||
{
|
||||
JointType = JointType.Pulley;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor for PulleyJoint.
|
||||
/// </summary>
|
||||
/// <param name="bodyA">The first body.</param>
|
||||
/// <param name="bodyB">The second body.</param>
|
||||
/// <param name="anchorA">The anchor on the first body.</param>
|
||||
/// <param name="anchorB">The anchor on the second body.</param>
|
||||
/// <param name="worldAnchorA">The world anchor for the first body.</param>
|
||||
/// <param name="worldAnchorB">The world anchor for the second body.</param>
|
||||
/// <param name="ratio">The ratio.</param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public PulleyJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Pulley;
|
||||
|
||||
WorldAnchorA = worldAnchorA;
|
||||
WorldAnchorB = worldAnchorB;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
{
|
||||
LocalAnchorA = BodyA.GetLocalPoint(anchorA);
|
||||
LocalAnchorB = BodyB.GetLocalPoint(anchorB);
|
||||
|
||||
Vector2 dA = anchorA - worldAnchorA;
|
||||
LengthA = dA.Length();
|
||||
Vector2 dB = anchorB - worldAnchorB;
|
||||
LengthB = dB.Length();
|
||||
}
|
||||
else
|
||||
{
|
||||
LocalAnchorA = anchorA;
|
||||
LocalAnchorB = anchorB;
|
||||
|
||||
Vector2 dA = anchorA - BodyA.GetLocalPoint(worldAnchorA);
|
||||
LengthA = dA.Length();
|
||||
Vector2 dB = anchorB - BodyB.GetLocalPoint(worldAnchorB);
|
||||
LengthB = dB.Length();
|
||||
}
|
||||
|
||||
Debug.Assert(ratio != 0.0f);
|
||||
Debug.Assert(ratio > Settings.Epsilon);
|
||||
|
||||
Ratio = ratio;
|
||||
Constant = LengthA + ratio * LengthB;
|
||||
_impulse = 0.0f;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyB
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorB { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the first world anchor.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public override sealed Vector2 WorldAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the second world anchor.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public override sealed Vector2 WorldAnchorB { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the current length of the segment attached to body1.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public float LengthA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get the current length of the segment attached to body2.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public float LengthB { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The current length between the anchor point on BodyA and WorldAnchorA
|
||||
/// </summary>
|
||||
public float CurrentLengthA
|
||||
{
|
||||
get
|
||||
{
|
||||
Vector2 p = BodyA.GetWorldPoint(LocalAnchorA);
|
||||
Vector2 s = WorldAnchorA;
|
||||
Vector2 d = p - s;
|
||||
return d.Length();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The current length between the anchor point on BodyB and WorldAnchorB
|
||||
/// </summary>
|
||||
public float CurrentLengthB
|
||||
{
|
||||
get
|
||||
{
|
||||
Vector2 p = BodyB.GetWorldPoint(LocalAnchorB);
|
||||
Vector2 s = WorldAnchorB;
|
||||
Vector2 d = p - s;
|
||||
return d.Length();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the pulley ratio.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public float Ratio { get; set; }
|
||||
|
||||
//FPE note: Only used for serialization.
|
||||
internal float Constant { get; set; }
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
Vector2 P = _impulse * _uB;
|
||||
return invDt * P;
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
|
||||
// Get the pulley axes.
|
||||
_uA = cA + _rA - WorldAnchorA;
|
||||
_uB = cB + _rB - WorldAnchorB;
|
||||
|
||||
float lengthA = _uA.Length();
|
||||
float lengthB = _uB.Length();
|
||||
|
||||
if (lengthA > 10.0f * Settings.LinearSlop)
|
||||
{
|
||||
_uA *= 1.0f / lengthA;
|
||||
}
|
||||
else
|
||||
{
|
||||
_uA = Vector2.Zero;
|
||||
}
|
||||
|
||||
if (lengthB > 10.0f * Settings.LinearSlop)
|
||||
{
|
||||
_uB *= 1.0f / lengthB;
|
||||
}
|
||||
else
|
||||
{
|
||||
_uB = Vector2.Zero;
|
||||
}
|
||||
|
||||
// Compute effective mass.
|
||||
float ruA = MathUtils.Cross(_rA, _uA);
|
||||
float ruB = MathUtils.Cross(_rB, _uB);
|
||||
|
||||
float mA = _invMassA + _invIA * ruA * ruA;
|
||||
float mB = _invMassB + _invIB * ruB * ruB;
|
||||
|
||||
_mass = mA + Ratio * Ratio * mB;
|
||||
|
||||
if (_mass > 0.0f)
|
||||
{
|
||||
_mass = 1.0f / _mass;
|
||||
}
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
// Scale impulses to support variable time steps.
|
||||
_impulse *= data.step.dtRatio;
|
||||
|
||||
// Warm starting.
|
||||
Vector2 PA = -(_impulse) * _uA;
|
||||
Vector2 PB = (-Ratio * _impulse) * _uB;
|
||||
|
||||
vA += _invMassA * PA;
|
||||
wA += _invIA * MathUtils.Cross(_rA, PA);
|
||||
vB += _invMassB * PB;
|
||||
wB += _invIB * MathUtils.Cross(_rB, PB);
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
|
||||
Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
|
||||
|
||||
float Cdot = -Vector2.Dot(_uA, vpA) - Ratio * Vector2.Dot(_uB, vpB);
|
||||
float impulse = -_mass * Cdot;
|
||||
_impulse += impulse;
|
||||
|
||||
Vector2 PA = -impulse * _uA;
|
||||
Vector2 PB = -Ratio * impulse * _uB;
|
||||
vA += _invMassA * PA;
|
||||
wA += _invIA * MathUtils.Cross(_rA, PA);
|
||||
vB += _invMassB * PB;
|
||||
wB += _invIB * MathUtils.Cross(_rB, PB);
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
|
||||
// Get the pulley axes.
|
||||
Vector2 uA = cA + rA - WorldAnchorA;
|
||||
Vector2 uB = cB + rB - WorldAnchorB;
|
||||
|
||||
float lengthA = uA.Length();
|
||||
float lengthB = uB.Length();
|
||||
|
||||
if (lengthA > 10.0f * Settings.LinearSlop)
|
||||
{
|
||||
uA *= 1.0f / lengthA;
|
||||
}
|
||||
else
|
||||
{
|
||||
uA = Vector2.Zero;
|
||||
}
|
||||
|
||||
if (lengthB > 10.0f * Settings.LinearSlop)
|
||||
{
|
||||
uB *= 1.0f / lengthB;
|
||||
}
|
||||
else
|
||||
{
|
||||
uB = Vector2.Zero;
|
||||
}
|
||||
|
||||
// Compute effective mass.
|
||||
float ruA = MathUtils.Cross(rA, uA);
|
||||
float ruB = MathUtils.Cross(rB, uB);
|
||||
|
||||
float mA = _invMassA + _invIA * ruA * ruA;
|
||||
float mB = _invMassB + _invIB * ruB * ruB;
|
||||
|
||||
float mass = mA + Ratio * Ratio * mB;
|
||||
|
||||
if (mass > 0.0f)
|
||||
{
|
||||
mass = 1.0f / mass;
|
||||
}
|
||||
|
||||
float C = Constant - lengthA - Ratio * lengthB;
|
||||
float linearError = Math.Abs(C);
|
||||
|
||||
float impulse = -mass * C;
|
||||
|
||||
Vector2 PA = -impulse * uA;
|
||||
Vector2 PB = -Ratio * impulse * uB;
|
||||
|
||||
cA += _invMassA * PA;
|
||||
aA += _invIA * MathUtils.Cross(rA, PA);
|
||||
cB += _invMassB * PB;
|
||||
aB += _invIB * MathUtils.Cross(rB, PB);
|
||||
|
||||
data.positions[_indexA].c = cA;
|
||||
data.positions[_indexA].a = aA;
|
||||
data.positions[_indexB].c = cB;
|
||||
data.positions[_indexB].a = aB;
|
||||
|
||||
return linearError < Settings.LinearSlop;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,619 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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 FarseerPhysics.Common;
|
||||
using Microsoft.Xna.Framework;
|
||||
|
||||
namespace FarseerPhysics.Dynamics.Joints
|
||||
{
|
||||
/// <summary>
|
||||
/// A revolute joint constrains 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
|
||||
{
|
||||
// Solver shared
|
||||
private Vector3 _impulse;
|
||||
private float _motorImpulse;
|
||||
|
||||
private bool _enableMotor;
|
||||
private float _maxMotorTorque;
|
||||
private float _motorSpeed;
|
||||
|
||||
private bool _enableLimit;
|
||||
private float _referenceAngle;
|
||||
private float _lowerAngle;
|
||||
private float _upperAngle;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _rA;
|
||||
private Vector2 _rB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
private Mat33 _mass; // effective mass for point-to-point constraint.
|
||||
private float _motorMass; // effective mass for motor/limit angular constraint.
|
||||
private LimitState _limitState;
|
||||
|
||||
internal RevoluteJoint()
|
||||
{
|
||||
JointType = JointType.Revolute;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor of RevoluteJoint.
|
||||
/// </summary>
|
||||
/// <param name="bodyA">The first body.</param>
|
||||
/// <param name="bodyB">The second body.</param>
|
||||
/// <param name="anchorA">The first body anchor.</param>
|
||||
/// <param name="anchorB">The second anchor.</param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public RevoluteJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Revolute;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
{
|
||||
LocalAnchorA = BodyA.GetLocalPoint(anchorA);
|
||||
LocalAnchorB = BodyB.GetLocalPoint(anchorB);
|
||||
}
|
||||
else
|
||||
{
|
||||
LocalAnchorA = anchorA;
|
||||
LocalAnchorB = anchorB;
|
||||
}
|
||||
|
||||
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
|
||||
|
||||
_impulse = Vector3.Zero;
|
||||
_limitState = LimitState.Inactive;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor of RevoluteJoint.
|
||||
/// </summary>
|
||||
/// <param name="bodyA">The first body.</param>
|
||||
/// <param name="bodyB">The second body.</param>
|
||||
/// <param name="anchor">The shared anchor.</param>
|
||||
/// <param name="useWorldCoordinates"></param>
|
||||
public RevoluteJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false)
|
||||
: this(bodyA, bodyB, anchor, anchor, useWorldCoordinates)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyB
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorB { get; set; }
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
||||
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorB
|
||||
{
|
||||
get { return BodyB.GetWorldPoint(LocalAnchorB); }
|
||||
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The referance angle computed as BodyB angle minus BodyA angle.
|
||||
/// </summary>
|
||||
public float ReferenceAngle
|
||||
{
|
||||
get { return _referenceAngle; }
|
||||
set
|
||||
{
|
||||
WakeBodies();
|
||||
_referenceAngle = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the current joint angle in radians.
|
||||
/// </summary>
|
||||
public float JointAngle
|
||||
{
|
||||
get { return BodyB._sweep.A - BodyA._sweep.A - ReferenceAngle; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the current joint angle speed in radians per second.
|
||||
/// </summary>
|
||||
public float JointSpeed
|
||||
{
|
||||
get { return BodyB._angularVelocity - BodyA._angularVelocity; }
|
||||
}
|
||||
|
||||
/// <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
|
||||
{
|
||||
if (_enableLimit != value)
|
||||
{
|
||||
WakeBodies();
|
||||
_enableLimit = value;
|
||||
_impulse.Z = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the lower joint limit in radians.
|
||||
/// </summary>
|
||||
public float LowerLimit
|
||||
{
|
||||
get { return _lowerAngle; }
|
||||
set
|
||||
{
|
||||
if (_lowerAngle != value)
|
||||
{
|
||||
WakeBodies();
|
||||
_lowerAngle = value;
|
||||
_impulse.Z = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the upper joint limit in radians.
|
||||
/// </summary>
|
||||
public float UpperLimit
|
||||
{
|
||||
get { return _upperAngle; }
|
||||
set
|
||||
{
|
||||
if (_upperAngle != value)
|
||||
{
|
||||
WakeBodies();
|
||||
_upperAngle = value;
|
||||
_impulse.Z = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Set the joint limits, usually in meters.
|
||||
/// </summary>
|
||||
/// <param name="lower">The lower limit</param>
|
||||
/// <param name="upper">The upper limit</param>
|
||||
public void SetLimits(float lower, float upper)
|
||||
{
|
||||
if (lower != _lowerAngle || upper != _upperAngle)
|
||||
{
|
||||
WakeBodies();
|
||||
_upperAngle = upper;
|
||||
_lowerAngle = lower;
|
||||
_impulse.Z = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/// <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>
|
||||
/// Get or set the motor speed in radians per second.
|
||||
/// </summary>
|
||||
public float MotorSpeed
|
||||
{
|
||||
set
|
||||
{
|
||||
WakeBodies();
|
||||
_motorSpeed = value;
|
||||
}
|
||||
get { return _motorSpeed; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get or set the maximum motor torque, usually in N-m.
|
||||
/// </summary>
|
||||
public float MaxMotorTorque
|
||||
{
|
||||
set
|
||||
{
|
||||
WakeBodies();
|
||||
_maxMotorTorque = value;
|
||||
}
|
||||
get { return _maxMotorTorque; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get or set the current motor impulse, usually in N-m.
|
||||
/// </summary>
|
||||
public float MotorImpulse
|
||||
{
|
||||
get { return _motorImpulse; }
|
||||
set
|
||||
{
|
||||
WakeBodies();
|
||||
_motorImpulse = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the motor torque in N-m.
|
||||
/// </summary>
|
||||
/// <param name="invDt">The inverse delta time</param>
|
||||
public float GetMotorTorque(float invDt)
|
||||
{
|
||||
return invDt * _motorImpulse;
|
||||
}
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
Vector2 p = new Vector2(_impulse.X, _impulse.Y);
|
||||
return invDt * p;
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return invDt * _impulse.Z;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
|
||||
// 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 = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
bool fixedRotation = (iA + iB == 0.0f);
|
||||
|
||||
_mass.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
|
||||
_mass.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
|
||||
_mass.ez.X = -_rA.Y * iA - _rB.Y * iB;
|
||||
_mass.ex.Y = _mass.ey.X;
|
||||
_mass.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
|
||||
_mass.ez.Y = _rA.X * iA + _rB.X * iB;
|
||||
_mass.ex.Z = _mass.ez.X;
|
||||
_mass.ey.Z = _mass.ez.Y;
|
||||
_mass.ez.Z = iA + iB;
|
||||
|
||||
_motorMass = iA + iB;
|
||||
if (_motorMass > 0.0f)
|
||||
{
|
||||
_motorMass = 1.0f / _motorMass;
|
||||
}
|
||||
|
||||
if (_enableMotor == false || fixedRotation)
|
||||
{
|
||||
_motorImpulse = 0.0f;
|
||||
}
|
||||
|
||||
if (_enableLimit && fixedRotation == false)
|
||||
{
|
||||
float jointAngle = aB - aA - 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 *= data.step.dtRatio;
|
||||
_motorImpulse *= data.step.dtRatio;
|
||||
|
||||
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z);
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = Vector3.Zero;
|
||||
_motorImpulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
bool fixedRotation = (iA + iB == 0.0f);
|
||||
|
||||
// Solve motor constraint.
|
||||
if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false)
|
||||
{
|
||||
float Cdot = wB - wA - _motorSpeed;
|
||||
float impulse = _motorMass * (-Cdot);
|
||||
float oldImpulse = _motorImpulse;
|
||||
float maxImpulse = data.step.dt * _maxMotorTorque;
|
||||
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
||||
impulse = _motorImpulse - oldImpulse;
|
||||
|
||||
wA -= iA * impulse;
|
||||
wB += iB * impulse;
|
||||
}
|
||||
|
||||
// Solve limit constraint.
|
||||
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
|
||||
{
|
||||
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);
|
||||
|
||||
if (_limitState == LimitState.Equal)
|
||||
{
|
||||
_impulse += impulse;
|
||||
}
|
||||
else if (_limitState == LimitState.AtLower)
|
||||
{
|
||||
float newImpulse = _impulse.Z + impulse.Z;
|
||||
if (newImpulse < 0.0f)
|
||||
{
|
||||
Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y);
|
||||
Vector2 reduced = _mass.Solve22(rhs);
|
||||
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
|
||||
{
|
||||
_impulse += impulse;
|
||||
}
|
||||
}
|
||||
else if (_limitState == LimitState.AtUpper)
|
||||
{
|
||||
float newImpulse = _impulse.Z + impulse.Z;
|
||||
if (newImpulse > 0.0f)
|
||||
{
|
||||
Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y);
|
||||
Vector2 reduced = _mass.Solve22(rhs);
|
||||
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
|
||||
{
|
||||
_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);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Solve point-to-point constraint
|
||||
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
|
||||
Vector2 impulse = _mass.Solve22(-Cdot);
|
||||
|
||||
_impulse.X += impulse.X;
|
||||
_impulse.Y += impulse.Y;
|
||||
|
||||
vA -= mA * impulse;
|
||||
wA -= iA * MathUtils.Cross(_rA, impulse);
|
||||
|
||||
vB += mB * impulse;
|
||||
wB += iB * MathUtils.Cross(_rB, impulse);
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
float angularError = 0.0f;
|
||||
float positionError;
|
||||
|
||||
bool fixedRotation = (_invIA + _invIB == 0.0f);
|
||||
|
||||
// Solve angular limit constraint.
|
||||
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
|
||||
{
|
||||
float angle = aB - aA - 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;
|
||||
}
|
||||
|
||||
aA -= _invIA * limitImpulse;
|
||||
aB += _invIB * limitImpulse;
|
||||
}
|
||||
|
||||
// Solve point-to-point constraint.
|
||||
{
|
||||
qA.Set(aA);
|
||||
qB.Set(aB);
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
|
||||
Vector2 C = cB + rB - cA - rA;
|
||||
positionError = C.Length();
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
Mat22 K = new Mat22();
|
||||
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
|
||||
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
|
||||
K.ey.X = K.ex.Y;
|
||||
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
|
||||
|
||||
Vector2 impulse = -K.Solve(C);
|
||||
|
||||
cA -= mA * impulse;
|
||||
aA -= iA * MathUtils.Cross(rA, impulse);
|
||||
|
||||
cB += mB * impulse;
|
||||
aB += iB * MathUtils.Cross(rB, impulse);
|
||||
}
|
||||
|
||||
data.positions[_indexA].c = cA;
|
||||
data.positions[_indexA].a = aA;
|
||||
data.positions[_indexB].c = cB;
|
||||
data.positions[_indexB].a = aB;
|
||||
|
||||
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,291 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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 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.
|
||||
/// It can be used on ropes that are made up of several connected bodies, and if there is a need to support a heavy body.
|
||||
/// This joint is used for stabiliation of heavy objects on soft constraint joints.
|
||||
///
|
||||
/// Warning: if you attempt to change the maximum length during the simulation you will get some non-physical behavior.
|
||||
/// Use the DistanceJoint instead if you want to dynamically control the length.
|
||||
/// </summary>
|
||||
public class RopeJoint : Joint
|
||||
{
|
||||
// Solver shared
|
||||
private float _impulse;
|
||||
private float _length;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
private float _mass;
|
||||
private Vector2 _rA, _rB;
|
||||
private Vector2 _u;
|
||||
|
||||
internal RopeJoint()
|
||||
{
|
||||
JointType = JointType.Rope;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor for RopeJoint.
|
||||
/// </summary>
|
||||
/// <param name="bodyA">The first body</param>
|
||||
/// <param name="bodyB">The second body</param>
|
||||
/// <param name="anchorA">The anchor on the first body</param>
|
||||
/// <param name="anchorB">The anchor on the second body</param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public RopeJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Rope;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
{
|
||||
LocalAnchorA = bodyA.GetLocalPoint(anchorA);
|
||||
LocalAnchorB = bodyB.GetLocalPoint(anchorB);
|
||||
}
|
||||
else
|
||||
{
|
||||
LocalAnchorA = anchorA;
|
||||
LocalAnchorB = anchorB;
|
||||
}
|
||||
|
||||
//FPE feature: Setting default MaxLength
|
||||
Vector2 d = WorldAnchorB - WorldAnchorA;
|
||||
MaxLength = d.Length();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyB
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorB { get; set; }
|
||||
|
||||
public override sealed Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
||||
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
public override sealed Vector2 WorldAnchorB
|
||||
{
|
||||
get { return BodyB.GetWorldPoint(LocalAnchorB); }
|
||||
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get or set the maximum length of the rope.
|
||||
/// By default, it is the distance between the two anchor points.
|
||||
/// </summary>
|
||||
public float MaxLength { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the state of the joint.
|
||||
/// </summary>
|
||||
public LimitState State { get; private set; }
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
return (invDt * _impulse) * _u;
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
_u = cB + _rB - cA - _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 = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;
|
||||
|
||||
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
// Scale the impulse to support a variable time step.
|
||||
_impulse *= data.step.dtRatio;
|
||||
|
||||
Vector2 P = _impulse * _u;
|
||||
vA -= _invMassA * P;
|
||||
wA -= _invIA * MathUtils.Cross(_rA, P);
|
||||
vB += _invMassB * P;
|
||||
wB += _invIB * MathUtils.Cross(_rB, P);
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
// Cdot = dot(u, v + cross(w, r))
|
||||
Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
|
||||
Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
|
||||
float C = _length - MaxLength;
|
||||
float Cdot = Vector2.Dot(_u, vpB - vpA);
|
||||
|
||||
// Predictive constraint.
|
||||
if (C < 0.0f)
|
||||
{
|
||||
Cdot += data.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;
|
||||
vA -= _invMassA * P;
|
||||
wA -= _invIA * MathUtils.Cross(_rA, P);
|
||||
vB += _invMassB * P;
|
||||
wB += _invIB * MathUtils.Cross(_rB, P);
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
Vector2 u = cB + rB - cA - 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;
|
||||
|
||||
cA -= _invMassA * P;
|
||||
aA -= _invIA * MathUtils.Cross(rA, P);
|
||||
cB += _invMassB * P;
|
||||
aB += _invIB * MathUtils.Cross(rB, P);
|
||||
|
||||
data.positions[_indexA].c = cA;
|
||||
data.positions[_indexA].a = aA;
|
||||
data.positions[_indexB].c = cB;
|
||||
data.positions[_indexB].a = aB;
|
||||
|
||||
return length - MaxLength < Settings.LinearSlop;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,388 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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 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.
|
||||
///
|
||||
/// The joint is soft constraint based, which means the two bodies will move
|
||||
/// relative to each other, when a force is applied. To combine two bodies
|
||||
/// in a rigid fashion, combine the fixtures to a single body instead.
|
||||
/// </summary>
|
||||
public class WeldJoint : Joint
|
||||
{
|
||||
// Solver shared
|
||||
private Vector3 _impulse;
|
||||
private float _gamma;
|
||||
private float _bias;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _rA;
|
||||
private Vector2 _rB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
private Mat33 _mass;
|
||||
|
||||
internal WeldJoint()
|
||||
{
|
||||
JointType = JointType.Weld;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// You need to specify an anchor point where they are attached.
|
||||
/// The position of the anchor point is important for computing the reaction torque.
|
||||
/// </summary>
|
||||
/// <param name="bodyA">The first body</param>
|
||||
/// <param name="bodyB">The second body</param>
|
||||
/// <param name="anchorA">The first body anchor.</param>
|
||||
/// <param name="anchorB">The second body anchor.</param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public WeldJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Weld;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
{
|
||||
LocalAnchorA = bodyA.GetLocalPoint(anchorA);
|
||||
LocalAnchorB = bodyB.GetLocalPoint(anchorB);
|
||||
}
|
||||
else
|
||||
{
|
||||
LocalAnchorA = anchorA;
|
||||
LocalAnchorB = anchorB;
|
||||
}
|
||||
|
||||
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyB
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorB { get; set; }
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
||||
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorB
|
||||
{
|
||||
get { return BodyB.GetWorldPoint(LocalAnchorB); }
|
||||
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The bodyB angle minus bodyA angle in the reference state (radians).
|
||||
/// </summary>
|
||||
public float ReferenceAngle { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The frequency of the joint. A higher frequency means a stiffer joint, but
|
||||
/// a too high value can cause the joint to oscillate.
|
||||
/// Default is 0, which means the joint does no spring calculations.
|
||||
/// </summary>
|
||||
public float FrequencyHz { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The damping on the joint. The damping is only used when
|
||||
/// the joint has a frequency (> 0). A higher value means more damping.
|
||||
/// </summary>
|
||||
public float DampingRatio { get; set; }
|
||||
|
||||
public override Vector2 GetReactionForce(float invDt)
|
||||
{
|
||||
return invDt * new Vector2(_impulse.X, _impulse.Y);
|
||||
}
|
||||
|
||||
public override float GetReactionTorque(float invDt)
|
||||
{
|
||||
return invDt * _impulse.Z;
|
||||
}
|
||||
|
||||
internal override void InitVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
|
||||
// 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 = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
Mat33 K = new Mat33();
|
||||
K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
|
||||
K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
|
||||
K.ez.X = -_rA.Y * iA - _rB.Y * iB;
|
||||
K.ex.Y = K.ey.X;
|
||||
K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
|
||||
K.ez.Y = _rA.X * iA + _rB.X * iB;
|
||||
K.ex.Z = K.ez.X;
|
||||
K.ey.Z = K.ez.Y;
|
||||
K.ez.Z = iA + iB;
|
||||
|
||||
if (FrequencyHz > 0.0f)
|
||||
{
|
||||
K.GetInverse22(ref _mass);
|
||||
|
||||
float invM = iA + iB;
|
||||
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
|
||||
|
||||
float C = aB - aA - ReferenceAngle;
|
||||
|
||||
// Frequency
|
||||
float omega = 2.0f * Settings.Pi * FrequencyHz;
|
||||
|
||||
// Damping coefficient
|
||||
float d = 2.0f * m * DampingRatio * omega;
|
||||
|
||||
// Spring stiffness
|
||||
float k = m * omega * omega;
|
||||
|
||||
// magic formulas
|
||||
float h = data.step.dt;
|
||||
_gamma = h * (d + h * k);
|
||||
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
|
||||
_bias = C * h * k * _gamma;
|
||||
|
||||
invM += _gamma;
|
||||
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
K.GetSymInverse33(ref _mass);
|
||||
_gamma = 0.0f;
|
||||
_bias = 0.0f;
|
||||
}
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
// Scale impulses to support a variable time step.
|
||||
_impulse *= data.step.dtRatio;
|
||||
|
||||
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);
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = Vector3.Zero;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
if (FrequencyHz > 0.0f)
|
||||
{
|
||||
float Cdot2 = wB - wA;
|
||||
|
||||
float impulse2 = -_mass.ez.Z * (Cdot2 + _bias + _gamma * _impulse.Z);
|
||||
_impulse.Z += impulse2;
|
||||
|
||||
wA -= iA * impulse2;
|
||||
wB += iB * impulse2;
|
||||
|
||||
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
|
||||
|
||||
Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1);
|
||||
_impulse.X += impulse1.X;
|
||||
_impulse.Y += impulse1.Y;
|
||||
|
||||
Vector2 P = impulse1;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * MathUtils.Cross(_rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * MathUtils.Cross(_rB, P);
|
||||
}
|
||||
else
|
||||
{
|
||||
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 = -MathUtils.Mul(_mass, 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);
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
|
||||
float positionError, angularError;
|
||||
|
||||
Mat33 K = new Mat33();
|
||||
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
|
||||
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
|
||||
K.ez.X = -rA.Y * iA - rB.Y * iB;
|
||||
K.ex.Y = K.ey.X;
|
||||
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
|
||||
K.ez.Y = rA.X * iA + rB.X * iB;
|
||||
K.ex.Z = K.ez.X;
|
||||
K.ey.Z = K.ez.Y;
|
||||
K.ez.Z = iA + iB;
|
||||
|
||||
if (FrequencyHz > 0.0f)
|
||||
{
|
||||
Vector2 C1 = cB + rB - cA - rA;
|
||||
|
||||
positionError = C1.Length();
|
||||
angularError = 0.0f;
|
||||
|
||||
Vector2 P = -K.Solve22(C1);
|
||||
|
||||
cA -= mA * P;
|
||||
aA -= iA * MathUtils.Cross(rA, P);
|
||||
|
||||
cB += mB * P;
|
||||
aB += iB * MathUtils.Cross(rB, P);
|
||||
}
|
||||
else
|
||||
{
|
||||
Vector2 C1 = cB + rB - cA - rA;
|
||||
float C2 = aB - aA - ReferenceAngle;
|
||||
|
||||
positionError = C1.Length();
|
||||
angularError = Math.Abs(C2);
|
||||
|
||||
Vector3 C = new Vector3(C1.X, C1.Y, C2);
|
||||
|
||||
Vector3 impulse = -K.Solve33(C);
|
||||
Vector2 P = new Vector2(impulse.X, impulse.Y);
|
||||
|
||||
cA -= mA * P;
|
||||
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
|
||||
|
||||
cB += mB * P;
|
||||
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
|
||||
}
|
||||
|
||||
data.positions[_indexA].c = cA;
|
||||
data.positions[_indexA].a = aA;
|
||||
data.positions[_indexB].c = cB;
|
||||
data.positions[_indexB].a = aB;
|
||||
|
||||
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,513 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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 FarseerPhysics.Common;
|
||||
using Microsoft.Xna.Framework;
|
||||
|
||||
namespace FarseerPhysics.Dynamics.Joints
|
||||
{
|
||||
// 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]
|
||||
|
||||
/// <summary>
|
||||
/// A wheel joint. This joint provides two degrees of freedom: translation
|
||||
/// along an axis fixed in bodyA and rotation in the plane. You can use a
|
||||
/// joint limit to restrict the range of motion and a joint motor to drive
|
||||
/// the rotation or to model rotational friction.
|
||||
/// This joint is designed for vehicle suspensions.
|
||||
/// </summary>
|
||||
public class WheelJoint : Joint
|
||||
{
|
||||
// Solver shared
|
||||
private Vector2 _localYAxis;
|
||||
|
||||
private float _impulse;
|
||||
private float _motorImpulse;
|
||||
private float _springImpulse;
|
||||
|
||||
private float _maxMotorTorque;
|
||||
private float _motorSpeed;
|
||||
private bool _enableMotor;
|
||||
|
||||
// Solver temp
|
||||
private int _indexA;
|
||||
private int _indexB;
|
||||
private Vector2 _localCenterA;
|
||||
private Vector2 _localCenterB;
|
||||
private float _invMassA;
|
||||
private float _invMassB;
|
||||
private float _invIA;
|
||||
private float _invIB;
|
||||
|
||||
private Vector2 _ax, _ay;
|
||||
private float _sAx, _sBx;
|
||||
private float _sAy, _sBy;
|
||||
|
||||
private float _mass;
|
||||
private float _motorMass;
|
||||
private float _springMass;
|
||||
|
||||
private float _bias;
|
||||
private float _gamma;
|
||||
private Vector2 _axis;
|
||||
|
||||
internal WheelJoint()
|
||||
{
|
||||
JointType = JointType.Wheel;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor for WheelJoint
|
||||
/// </summary>
|
||||
/// <param name="bodyA">The first body</param>
|
||||
/// <param name="bodyB">The second body</param>
|
||||
/// <param name="anchor">The anchor point</param>
|
||||
/// <param name="axis">The axis</param>
|
||||
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
|
||||
public WheelJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false)
|
||||
: base(bodyA, bodyB)
|
||||
{
|
||||
JointType = JointType.Wheel;
|
||||
|
||||
if (useWorldCoordinates)
|
||||
{
|
||||
LocalAnchorA = bodyA.GetLocalPoint(anchor);
|
||||
LocalAnchorB = bodyB.GetLocalPoint(anchor);
|
||||
}
|
||||
else
|
||||
{
|
||||
LocalAnchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchor));
|
||||
LocalAnchorB = anchor;
|
||||
}
|
||||
|
||||
Axis = axis; //FPE only: We maintain the original value as it is supposed to.
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorA { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// The local anchor point on BodyB
|
||||
/// </summary>
|
||||
public Vector2 LocalAnchorB { get; set; }
|
||||
|
||||
public override Vector2 WorldAnchorA
|
||||
{
|
||||
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
||||
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
public override Vector2 WorldAnchorB
|
||||
{
|
||||
get { return BodyB.GetWorldPoint(LocalAnchorB); }
|
||||
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The axis at which the suspension moves.
|
||||
/// </summary>
|
||||
public Vector2 Axis
|
||||
{
|
||||
get { return _axis; }
|
||||
set
|
||||
{
|
||||
_axis = value;
|
||||
LocalXAxis = BodyA.GetLocalVector(_axis);
|
||||
_localYAxis = MathUtils.Cross(1.0f, LocalXAxis);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The axis in local coordinates relative to BodyA
|
||||
/// </summary>
|
||||
public Vector2 LocalXAxis { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// The desired motor speed in radians per second.
|
||||
/// </summary>
|
||||
public float MotorSpeed
|
||||
{
|
||||
get { return _motorSpeed; }
|
||||
set
|
||||
{
|
||||
WakeBodies();
|
||||
_motorSpeed = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The maximum motor torque, usually in N-m.
|
||||
/// </summary>
|
||||
public float MaxMotorTorque
|
||||
{
|
||||
get { return _maxMotorTorque; }
|
||||
set
|
||||
{
|
||||
WakeBodies();
|
||||
_maxMotorTorque = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Suspension frequency, zero indicates no suspension
|
||||
/// </summary>
|
||||
public float Frequency { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Suspension damping ratio, one indicates critical damping
|
||||
/// </summary>
|
||||
public float DampingRatio { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the translation along the axis
|
||||
/// </summary>
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the angular velocity of the joint
|
||||
/// </summary>
|
||||
public float JointSpeed
|
||||
{
|
||||
get
|
||||
{
|
||||
float wA = BodyA.AngularVelocity;
|
||||
float wB = BodyB.AngularVelocity;
|
||||
return wB - wA;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Enable/disable the joint motor.
|
||||
/// </summary>
|
||||
public bool MotorEnabled
|
||||
{
|
||||
get { return _enableMotor; }
|
||||
set
|
||||
{
|
||||
WakeBodies();
|
||||
_enableMotor = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the torque of the motor
|
||||
/// </summary>
|
||||
/// <param name="invDt">inverse delta time</param>
|
||||
public float GetMotorTorque(float invDt)
|
||||
{
|
||||
return invDt * _motorImpulse;
|
||||
}
|
||||
|
||||
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 SolverData data)
|
||||
{
|
||||
_indexA = BodyA.IslandIndex;
|
||||
_indexB = BodyB.IslandIndex;
|
||||
_localCenterA = BodyA._sweep.LocalCenter;
|
||||
_localCenterB = BodyB._sweep.LocalCenter;
|
||||
_invMassA = BodyA._invMass;
|
||||
_invMassB = BodyB._invMass;
|
||||
_invIA = BodyA._invI;
|
||||
_invIB = BodyB._invI;
|
||||
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
// Compute the effective masses.
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
Vector2 d1 = cB + rB - cA - rA;
|
||||
|
||||
// Point to line constraint
|
||||
{
|
||||
_ay = MathUtils.Mul(qA, _localYAxis);
|
||||
_sAy = MathUtils.Cross(d1 + rA, _ay);
|
||||
_sBy = MathUtils.Cross(rB, _ay);
|
||||
|
||||
_mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy;
|
||||
|
||||
if (_mass > 0.0f)
|
||||
{
|
||||
_mass = 1.0f / _mass;
|
||||
}
|
||||
}
|
||||
|
||||
// Spring constraint
|
||||
_springMass = 0.0f;
|
||||
_bias = 0.0f;
|
||||
_gamma = 0.0f;
|
||||
if (Frequency > 0.0f)
|
||||
{
|
||||
_ax = MathUtils.Mul(qA, LocalXAxis);
|
||||
_sAx = MathUtils.Cross(d1 + rA, _ax);
|
||||
_sBx = MathUtils.Cross(rB, _ax);
|
||||
|
||||
float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx;
|
||||
|
||||
if (invMass > 0.0f)
|
||||
{
|
||||
_springMass = 1.0f / invMass;
|
||||
|
||||
float C = Vector2.Dot(d1, _ax);
|
||||
|
||||
// Frequency
|
||||
float omega = 2.0f * Settings.Pi * Frequency;
|
||||
|
||||
// Damping coefficient
|
||||
float d = 2.0f * _springMass * DampingRatio * omega;
|
||||
|
||||
// Spring stiffness
|
||||
float k = _springMass * omega * omega;
|
||||
|
||||
// magic formulas
|
||||
float h = data.step.dt;
|
||||
_gamma = h * (d + h * k);
|
||||
if (_gamma > 0.0f)
|
||||
{
|
||||
_gamma = 1.0f / _gamma;
|
||||
}
|
||||
|
||||
_bias = C * h * k * _gamma;
|
||||
|
||||
_springMass = invMass + _gamma;
|
||||
if (_springMass > 0.0f)
|
||||
{
|
||||
_springMass = 1.0f / _springMass;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_springImpulse = 0.0f;
|
||||
}
|
||||
|
||||
// Rotational motor
|
||||
if (_enableMotor)
|
||||
{
|
||||
_motorMass = iA + iB;
|
||||
if (_motorMass > 0.0f)
|
||||
{
|
||||
_motorMass = 1.0f / _motorMass;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_motorMass = 0.0f;
|
||||
_motorImpulse = 0.0f;
|
||||
}
|
||||
|
||||
if (Settings.EnableWarmstarting)
|
||||
{
|
||||
// Account for variable time step.
|
||||
_impulse *= data.step.dtRatio;
|
||||
_springImpulse *= data.step.dtRatio;
|
||||
_motorImpulse *= data.step.dtRatio;
|
||||
|
||||
Vector2 P = _impulse * _ay + _springImpulse * _ax;
|
||||
float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse;
|
||||
float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
|
||||
|
||||
vA -= _invMassA * P;
|
||||
wA -= _invIA * LA;
|
||||
|
||||
vB += _invMassB * P;
|
||||
wB += _invIB * LB;
|
||||
}
|
||||
else
|
||||
{
|
||||
_impulse = 0.0f;
|
||||
_springImpulse = 0.0f;
|
||||
_motorImpulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override void SolveVelocityConstraints(ref SolverData data)
|
||||
{
|
||||
float mA = _invMassA, mB = _invMassB;
|
||||
float iA = _invIA, iB = _invIB;
|
||||
|
||||
Vector2 vA = data.velocities[_indexA].v;
|
||||
float wA = data.velocities[_indexA].w;
|
||||
Vector2 vB = data.velocities[_indexB].v;
|
||||
float wB = data.velocities[_indexB].w;
|
||||
|
||||
// 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 -= mA * P;
|
||||
wA -= iA * LA;
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * LB;
|
||||
}
|
||||
|
||||
// Solve rotational motor constraint
|
||||
{
|
||||
float Cdot = wB - wA - _motorSpeed;
|
||||
float impulse = -_motorMass * Cdot;
|
||||
|
||||
float oldImpulse = _motorImpulse;
|
||||
float maxImpulse = data.step.dt * _maxMotorTorque;
|
||||
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
||||
impulse = _motorImpulse - oldImpulse;
|
||||
|
||||
wA -= iA * impulse;
|
||||
wB += iB * 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 -= mA * P;
|
||||
wA -= iA * LA;
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * LB;
|
||||
}
|
||||
|
||||
data.velocities[_indexA].v = vA;
|
||||
data.velocities[_indexA].w = wA;
|
||||
data.velocities[_indexB].v = vB;
|
||||
data.velocities[_indexB].w = wB;
|
||||
}
|
||||
|
||||
internal override bool SolvePositionConstraints(ref SolverData data)
|
||||
{
|
||||
Vector2 cA = data.positions[_indexA].c;
|
||||
float aA = data.positions[_indexA].a;
|
||||
Vector2 cB = data.positions[_indexB].c;
|
||||
float aB = data.positions[_indexB].a;
|
||||
|
||||
Rot qA = new Rot(aA), qB = new Rot(aB);
|
||||
|
||||
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
|
||||
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
|
||||
Vector2 d = (cB - cA) + rB - rA;
|
||||
|
||||
Vector2 ay = MathUtils.Mul(qA, _localYAxis);
|
||||
|
||||
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;
|
||||
|
||||
cA -= _invMassA * P;
|
||||
aA -= _invIA * LA;
|
||||
cB += _invMassB * P;
|
||||
aB += _invIB * LB;
|
||||
|
||||
data.positions[_indexA].c = cA;
|
||||
data.positions[_indexA].a = aA;
|
||||
data.positions[_indexB].c = cB;
|
||||
data.positions[_indexB].a = aB;
|
||||
|
||||
return Math.Abs(C) <= Settings.LinearSlop;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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 Microsoft.Xna.Framework;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/// This is an internal structure.
|
||||
public struct Position
|
||||
{
|
||||
public Vector2 c;
|
||||
public float a;
|
||||
}
|
||||
|
||||
/// This is an internal structure.
|
||||
public struct Velocity
|
||||
{
|
||||
public Vector2 v;
|
||||
public float w;
|
||||
}
|
||||
|
||||
/// Solver Data
|
||||
public struct SolverData
|
||||
{
|
||||
public TimeStep step;
|
||||
public Position[] positions;
|
||||
public Velocity[] velocities;
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* Farseer Physics Engine:
|
||||
* Copyright (c) 2012 Ian Qvist
|
||||
*
|
||||
* Original source Box2D:
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* 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;
|
||||
|
||||
namespace FarseerPhysics.Dynamics
|
||||
{
|
||||
/// <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, ContactVelocityConstraint 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, ContactVelocityConstraint impulse);
|
||||
|
||||
public delegate void OnSeparationEventHandler(Fixture fixtureA, Fixture fixtureB);
|
||||
}
|
||||
Reference in New Issue
Block a user