vlad.os/LuaCsForBarotraumaEP
0
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

(61d00a474) v0.9.7.1

Browse Source
This commit is contained in:
Regalis
2020-03-04 13:04:10 +01:00
parent 3c50efa5c9
commit 3c09ebe02f
5086 changed files with 786063 additions and 295871 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Libraries/Farseer Physics Engine 3.5/Dynamics/Contacts/Contact.cs
+95 -76
View File
@@ -1,3 +1,10 @@
// Copyright (c) 2017 Kastellanos Nikolaos
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
@@ -19,7 +26,6 @@
* 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;
@@ -42,22 +48,22 @@ namespace FarseerPhysics.Dynamics.Contacts
/// <summary>
/// The contact
/// </summary>
public Contact Contact;
/// <summary>
/// The next contact edge in the body's contact list
/// </summary>
public ContactEdge Next;
public Contact Contact { get; internal set; }
/// <summary>
/// Provides quick access to the other body attached.
/// </summary>
public Body Other;
public Body Other { get; internal set; }
/// <summary>
/// The next contact edge in the body's contact list
/// </summary>
public ContactEdge Next { get; internal set; }
/// <summary>
/// The previous contact edge in the body's contact list
/// </summary>
public ContactEdge Prev;
public ContactEdge Prev { get; internal set; }
}
/// <summary>
@@ -108,8 +114,9 @@ namespace FarseerPhysics.Dynamics.Contacts
internal int _toiCount;
internal float _toi;
public Fixture FixtureA;
public Fixture FixtureB;
public Fixture FixtureA { get; internal set; }
public Fixture FixtureB { get; internal set; }
public float Friction { get; set; }
public float Restitution { get; set; }
@@ -142,6 +149,18 @@ namespace FarseerPhysics.Dynamics.Contacts
/// <value>The child index B.</value>
public int ChildIndexB { get; internal set; }
/// <summary>
/// Get the next contact in the world's contact list.
/// </summary>
/// <value>The next.</value>
public Contact Next { get; internal set; }
/// <summary>
/// Get the previous contact in the world's contact list.
/// </summary>
/// <value>The prev.</value>
public Contact Prev { get; internal set; }
/// <summary>
/// Determines whether this contact is touching.
/// </summary>
@@ -164,7 +183,7 @@ namespace FarseerPhysics.Dynamics.Contacts
Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction);
}
private Contact(Fixture fA, int indexA, Fixture fB, int indexB)
protected Contact(Fixture fA, int indexA, Fixture fB, int indexB)
{
Reset(fA, indexA, fB, indexB);
}
@@ -198,15 +217,18 @@ namespace FarseerPhysics.Dynamics.Contacts
Manifold.PointCount = 0;
Next = null;
Prev = null;
_nodeA.Contact = null;
_nodeA.Prev = null;
_nodeA.Next = null;
_nodeA.Other = null;
_nodeA.Next = null;
_nodeA.Prev = null;
_nodeB.Contact = null;
_nodeB.Prev = null;
_nodeB.Next = null;
_nodeB.Other = null;
_nodeB.Next = null;
_nodeB.Prev = null;
_toiCount = 0;
@@ -295,45 +317,37 @@ namespace FarseerPhysics.Dynamics.Contacts
{
if (touching)
{
if (Settings.AllCollisionCallbacksAgree)
{
bool enabledA = true, enabledB = true;
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;
// 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;
// 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;
// Report the collision to both bodies:
if (FixtureA.Body != null && FixtureA.Body.onCollisionEventHandler != null)
foreach (OnCollisionEventHandler handler in FixtureA.Body.onCollisionEventHandler.GetInvocationList())
enabledA = handler(FixtureA, FixtureB, this) && enabledA;
// 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.Body != null && FixtureB.Body.onCollisionEventHandler != null)
foreach (OnCollisionEventHandler handler in FixtureB.Body.onCollisionEventHandler.GetInvocationList())
enabledB = handler(FixtureB, FixtureA, this) && enabledB;
//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);
}
Enabled = enabledA && enabledB;
// BeginContact can also return false and disable the contact
if (enabledA && enabledB && 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
@@ -348,12 +362,22 @@ namespace FarseerPhysics.Dynamics.Contacts
{
//Report the separation to both participants:
if (FixtureA != null && FixtureA.OnSeparation != null)
FixtureA.OnSeparation(FixtureA, FixtureB);
FixtureA.OnSeparation(FixtureA, FixtureB, this);
//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);
FixtureB.OnSeparation(FixtureB, FixtureA, this);
//Report the separation to both bodies:
if (FixtureA != null && FixtureA.Body != null && FixtureA.Body.onSeparationEventHandler != null)
FixtureA.Body.onSeparationEventHandler(FixtureA, FixtureB, this);
//Reverse the order of the reported fixtures. The first fixture is always the one that the
//user subscribed to.
if (FixtureB != null && FixtureB.Body != null && FixtureB.Body.onSeparationEventHandler != null)
FixtureB.Body.onSeparationEventHandler(FixtureB, FixtureA, this);
if (contactManager.EndContact != null)
contactManager.EndContact(this);
@@ -405,7 +429,7 @@ namespace FarseerPhysics.Dynamics.Contacts
}
}
internal static Contact Create(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
internal static Contact Create(ContactManager contactManager, Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
{
ShapeType type1 = fixtureA.Shape.ShapeType;
ShapeType type2 = fixtureB.Shape.ShapeType;
@@ -413,32 +437,32 @@ namespace FarseerPhysics.Dynamics.Contacts
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)
Contact c = null;
var contactPoolList = contactManager._contactPoolList;
if (contactPoolList.Next != contactPoolList)
{
// get first item in the pool.
c = contactPoolList.Next;
// Remove from the pool.
contactPoolList.Next = c.Next;
c.Next = null;
}
// 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 = pool.Dequeue();
if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
{
c.Reset(fixtureA, indexA, fixtureB, indexB);
}
if (c == null)
c = new Contact(fixtureA, indexA, fixtureB, indexB);
else
{
c.Reset(fixtureB, indexB, fixtureA, indexA);
}
c.Reset(fixtureA, indexA, fixtureB, indexB);
}
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
{
if (c == null)
c = new Contact(fixtureB, indexB, fixtureA, indexA);
}
else
c.Reset(fixtureB, indexB, fixtureA, indexA);
}
c._type = _registers[(int)type1, (int)type2];
@@ -447,11 +471,6 @@ namespace FarseerPhysics.Dynamics.Contacts
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;
Libraries/Farseer Physics Engine 3.5/Dynamics/Contacts/ContactListHead.cs
+69
View File
@@ -0,0 +1,69 @@
// Copyright (c) 2017 Kastellanos Nikolaos
using System;
using System.Collections;
using System.Collections.Generic;
namespace FarseerPhysics.Dynamics.Contacts
{
/// <summary>
/// Head of a circular doubly linked list.
/// </summary>
public class ContactListHead : Contact , IEnumerable<Contact>
{
internal ContactListHead(): base(null, 0, null, 0)
{
this.Prev = this;
this.Next = this;
}
IEnumerator<Contact> IEnumerable<Contact>.GetEnumerator()
{
return new ContactEnumerator(this);
}
IEnumerator IEnumerable.GetEnumerator()
{
return new ContactEnumerator(this);
}
#region Nested type: ContactEnumerator
private struct ContactEnumerator : IEnumerator<Contact>
{
private ContactListHead _head;
private Contact _current;
public Contact Current { get { return _current; } }
object IEnumerator.Current { get { return _current; } }
public ContactEnumerator(ContactListHead contact)
{
_head = contact;
_current = _head;
}
public void Reset()
{
_current = _head;
}
public bool MoveNext()
{
_current = _current.Next;
return (_current != _head);
}
public void Dispose()
{
_head = null;
_current = null;
}
}
#endregion
}
}
Libraries/Farseer Physics Engine 3.5/Dynamics/Contacts/ContactSolver.cs
+276 -89
View File
@@ -1,3 +1,10 @@
// Copyright (c) 2017 Kastellanos Nikolaos
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
@@ -25,7 +32,12 @@ using System.Diagnostics;
using FarseerPhysics.Collision;
using FarseerPhysics.Collision.Shapes;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
using System.Threading;
using System.Threading.Tasks;
#endif
namespace FarseerPhysics.Dynamics.Contacts
{
@@ -82,21 +94,24 @@ namespace FarseerPhysics.Dynamics.Contacts
public class ContactSolver
{
public TimeStep _step;
public Position[] _positions;
public Velocity[] _velocities;
public ContactPositionConstraint[] _positionConstraints;
public ContactVelocityConstraint[] _velocityConstraints;
public Contact[] _contacts;
public int _count;
int _velocityConstraintsMultithreadThreshold;
int _positionConstraintsMultithreadThreshold;
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting)
public void Reset(ref TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities,
int velocityConstraintsMultithreadThreshold, int positionConstraintsMultithreadThreshold)
{
_step = step;
_count = count;
_positions = positions;
_velocities = velocities;
_contacts = contacts;
_velocityConstraintsMultithreadThreshold = velocityConstraintsMultithreadThreshold;
_positionConstraintsMultithreadThreshold = positionConstraintsMultithreadThreshold;
// grow the array
if (_velocityConstraints == null || _velocityConstraints.Length < count)
@@ -169,10 +184,10 @@ namespace FarseerPhysics.Dynamics.Contacts
ManifoldPoint cp = manifold.Points[j];
VelocityConstraintPoint vcp = vc.points[j];
if (Settings.EnableWarmstarting)
if (step.warmStarting)
{
vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse;
vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse;
vcp.normalImpulse = step.dtRatio * cp.NormalImpulse;
vcp.tangentImpulse = step.dtRatio * cp.TangentImpulse;
}
else
{
@@ -224,18 +239,17 @@ namespace FarseerPhysics.Dynamics.Contacts
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);
Transform xfA = new Transform(Vector2.Zero, aA);
Transform xfB = new Transform(Vector2.Zero, aB);
xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
Vector2 normal;
FixedArray2<Vector2> points;
WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points);
vc.normal = normal;
Vector2 tangent = MathUtils.Rot270(ref vc.normal);
int pointCount = vc.pointCount;
for (int j = 0; j < pointCount; ++j)
@@ -245,17 +259,16 @@ namespace FarseerPhysics.Dynamics.Contacts
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 rnA = MathUtils.Cross(ref vcp.rA, ref vc.normal);
float rnB = MathUtils.Cross(ref vcp.rB, ref 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 rtA = MathUtils.Cross(ref vcp.rA, ref tangent);
float rtB = MathUtils.Cross(ref vcp.rB, ref tangent);
float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
@@ -263,7 +276,7 @@ namespace FarseerPhysics.Dynamics.Contacts
// 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));
float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA));
if (vRel < -Settings.VelocityThreshold)
{
vcp.velocityBias = -vc.restitution * vRel;
@@ -276,10 +289,10 @@ namespace FarseerPhysics.Dynamics.Contacts
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 rn1A = MathUtils.Cross(ref vcp1.rA, ref vc.normal);
float rn1B = MathUtils.Cross(ref vcp1.rB, ref vc.normal);
float rn2A = MathUtils.Cross(ref vcp2.rA, ref vc.normal);
float rn2B = MathUtils.Cross(ref vcp2.rB, ref vc.normal);
float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
@@ -325,15 +338,15 @@ namespace FarseerPhysics.Dynamics.Contacts
float wB = _velocities[indexB].w;
Vector2 normal = vc.normal;
Vector2 tangent = MathUtils.Cross(normal, 1.0f);
Vector2 tangent = MathUtils.Rot270(ref normal);
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);
wA -= iA * MathUtils.Cross(ref vcp.rA, ref P);
vA -= mA * P;
wB += iB * MathUtils.Cross(vcp.rB, P);
wB += iB * MathUtils.Cross(ref vcp.rB, ref P);
vB += mB * P;
}
@@ -346,10 +359,115 @@ namespace FarseerPhysics.Dynamics.Contacts
public void SolveVelocityConstraints()
{
for (int i = 0; i < _count; ++i)
if (_count >= _velocityConstraintsMultithreadThreshold && System.Environment.ProcessorCount > 1)
{
if (_count == 0) return;
var batchSize = (int)Math.Ceiling((float)_count / System.Environment.ProcessorCount);
var batches = (int)Math.Ceiling((float)_count / batchSize);
#if NET40 || NET45
SolveVelocityConstraintsWaitLock.Reset(batches);
for (int i = 0; i < batches; i++)
{
var start = i * batchSize;
var end = Math.Min(start + batchSize, _count);
ThreadPool.QueueUserWorkItem( SolveVelocityConstraintsCallback, SolveVelocityConstraintsState.Get(this, start,end));
}
// We avoid SolveVelocityConstraintsWaitLock.Wait(); because it spins a few milliseconds before going into sleep. Going into sleep(0) directly in a while loop is faster.
while (SolveVelocityConstraintsWaitLock.CurrentCount > 0)
Thread.Sleep(0);
#elif PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
Parallel.For(0, batches, (i) =>
{
var start = i * batchSize;
var end = Math.Min(start + batchSize, _count);
SolveVelocityConstraints(start, end);
});
#else
SolveVelocityConstraints(0, _count);
#endif
}
else
{
SolveVelocityConstraints(0, _count);
}
return;
}
#if NET40 || NET45
CountdownEvent SolveVelocityConstraintsWaitLock = new CountdownEvent(0);
static void SolveVelocityConstraintsCallback(object state)
{
var svcState = (SolveVelocityConstraintsState)state;
svcState.ContactSolver.SolveVelocityConstraints(svcState.Start, svcState.End);
SolveVelocityConstraintsState.Return(svcState);
svcState.ContactSolver.SolveVelocityConstraintsWaitLock.Signal();
}
private class SolveVelocityConstraintsState
{
private static System.Collections.Concurrent.ConcurrentQueue<SolveVelocityConstraintsState> _queue = new System.Collections.Concurrent.ConcurrentQueue<SolveVelocityConstraintsState>(); // pool
public ContactSolver ContactSolver;
public int Start { get; private set; }
public int End { get; private set; }
private SolveVelocityConstraintsState()
{
}
internal static object Get(ContactSolver contactSolver, int start, int end)
{
SolveVelocityConstraintsState result;
if (!_queue.TryDequeue(out result))
result = new SolveVelocityConstraintsState();
result.ContactSolver = contactSolver;
result.Start = start;
result.End = end;
return result;
}
internal static void Return(object state)
{
_queue.Enqueue((SolveVelocityConstraintsState)state);
}
}
#endif
private void SolveVelocityConstraints(int start, int end)
{
for (int i = start; i < end; ++i)
{
ContactVelocityConstraint vc = _velocityConstraints[i];
#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
// find lower order item
int orderedIndexA = vc.indexA;
int orderedIndexB = vc.indexB;
if (orderedIndexB < orderedIndexA)
{
orderedIndexA = vc.indexB;
orderedIndexB = vc.indexA;
}
for (; ; )
{
if (Interlocked.CompareExchange(ref _velocities[orderedIndexA].Lock, 1, 0) == 0)
{
if (Interlocked.CompareExchange(ref _velocities[orderedIndexB].Lock, 1, 0) == 0)
break;
System.Threading.Interlocked.Exchange(ref _velocities[orderedIndexA].Lock, 0);
}
#if NET40 || NET45
Thread.Sleep(0);
#endif
}
#endif
int indexA = vc.indexA;
int indexB = vc.indexB;
float mA = vc.invMassA;
@@ -364,7 +482,7 @@ namespace FarseerPhysics.Dynamics.Contacts
float wB = _velocities[indexB].w;
Vector2 normal = vc.normal;
Vector2 tangent = MathUtils.Cross(normal, 1.0f);
Vector2 tangent = MathUtils.Rot270(ref normal);
float friction = vc.friction;
Debug.Assert(pointCount == 1 || pointCount == 2);
@@ -376,7 +494,7 @@ namespace FarseerPhysics.Dynamics.Contacts
VelocityConstraintPoint vcp = vc.points[j];
// Relative velocity at contact
Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
Vector2 dv = vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA);
// Compute tangent force
float vt = Vector2.Dot(dv, tangent) - vc.tangentSpeed;
@@ -392,10 +510,10 @@ namespace FarseerPhysics.Dynamics.Contacts
Vector2 P = lambda * tangent;
vA -= mA * P;
wA -= iA * MathUtils.Cross(vcp.rA, P);
wA -= iA * MathUtils.Cross(ref vcp.rA, ref P);
vB += mB * P;
wB += iB * MathUtils.Cross(vcp.rB, P);
wB += iB * MathUtils.Cross(ref vcp.rB, ref P);
}
// Solve normal constraints
@@ -404,7 +522,7 @@ namespace FarseerPhysics.Dynamics.Contacts
VelocityConstraintPoint vcp = vc.points[0];
// Relative velocity at contact
Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
Vector2 dv = vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA);
// Compute normal impulse
float vn = Vector2.Dot(dv, normal);
@@ -418,10 +536,10 @@ namespace FarseerPhysics.Dynamics.Contacts
// Apply contact impulse
Vector2 P = lambda * normal;
vA -= mA * P;
wA -= iA * MathUtils.Cross(vcp.rA, P);
wA -= iA * MathUtils.Cross(ref vcp.rA, ref P);
vB += mB * P;
wB += iB * MathUtils.Cross(vcp.rB, P);
wB += iB * MathUtils.Cross(ref vcp.rB, ref P);
}
else
{
@@ -465,8 +583,8 @@ namespace FarseerPhysics.Dynamics.Contacts
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);
Vector2 dv1 = vB + MathUtils.Cross(wB, ref cp1.rB) - vA - MathUtils.Cross(wA, ref cp1.rA);
Vector2 dv2 = vB + MathUtils.Cross(wB, ref cp2.rB) - vA - MathUtils.Cross(wA, ref cp2.rA);
// Compute normal velocity
float vn1 = Vector2.Dot(dv1, normal);
@@ -477,7 +595,7 @@ namespace FarseerPhysics.Dynamics.Contacts
b.Y = vn2 - cp2.velocityBias;
// Compute b'
b -= MathUtils.Mul(ref vc.K, a);
b -= MathUtils.Mul(ref vc.K, ref a);
const float k_errorTol = 1e-3f;
//B2_NOT_USED(k_errorTol);
@@ -493,7 +611,7 @@ namespace FarseerPhysics.Dynamics.Contacts
//
// x = - inv(A) * b'
//
Vector2 x = -MathUtils.Mul(ref vc.normalMass, b);
Vector2 x = -MathUtils.Mul(ref vc.normalMass, ref b);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
@@ -504,10 +622,10 @@ namespace FarseerPhysics.Dynamics.Contacts
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));
wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
// Accumulate
cp1.normalImpulse = x.X;
@@ -548,10 +666,10 @@ namespace FarseerPhysics.Dynamics.Contacts
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));
wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
// Accumulate
cp1.normalImpulse = x.X;
@@ -590,10 +708,10 @@ namespace FarseerPhysics.Dynamics.Contacts
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));
wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
// Accumulate
cp1.normalImpulse = x.X;
@@ -630,10 +748,10 @@ namespace FarseerPhysics.Dynamics.Contacts
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));
wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
// Accumulate
cp1.normalImpulse = x.X;
@@ -651,6 +769,11 @@ namespace FarseerPhysics.Dynamics.Contacts
_velocities[indexA].w = wA;
_velocities[indexB].v = vB;
_velocities[indexB].w = wB;
#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
System.Threading.Interlocked.Exchange(ref _velocities[orderedIndexB].Lock, 0);
System.Threading.Interlocked.Exchange(ref _velocities[orderedIndexA].Lock, 0);
#endif
}
}
@@ -674,12 +797,71 @@ namespace FarseerPhysics.Dynamics.Contacts
}
public bool SolvePositionConstraints()
{
bool contactsOkay = false;
if (_count >= _positionConstraintsMultithreadThreshold && System.Environment.ProcessorCount > 1)
{
if (_count == 0) return true;
var batchSize = (int)Math.Ceiling((float)_count / System.Environment.ProcessorCount);
var batches = (int)Math.Ceiling((float)_count / batchSize);
#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
Parallel.For(0, batches, (i) =>
{
var start = i * batchSize;
var end = Math.Min(start + batchSize, _count);
var res = SolvePositionConstraints(start, end);
lock (this)
{
contactsOkay = contactsOkay || res;
}
});
#else
contactsOkay = SolvePositionConstraints(0, _count);
#endif
}
else
{
contactsOkay = SolvePositionConstraints(0, _count);
}
return contactsOkay;
}
private bool SolvePositionConstraints(int start, int end)
{
float minSeparation = 0.0f;
for (int i = 0; i < _count; ++i)
for (int i = start; i < end; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
// Find lower order item.
int orderedIndexA = pc.indexA;
int orderedIndexB = pc.indexB;
if (orderedIndexB < orderedIndexA)
{
orderedIndexA = pc.indexB;
orderedIndexB = pc.indexA;
}
// Lock bodies.
for (; ; )
{
if (Interlocked.CompareExchange(ref _positions[orderedIndexA].Lock, 1, 0) == 0)
{
if (Interlocked.CompareExchange(ref _positions[orderedIndexB].Lock, 1, 0) == 0)
break;
System.Threading.Interlocked.Exchange(ref _positions[orderedIndexA].Lock, 0);
}
#if NET40 || NET45
Thread.Sleep(0);
#endif
}
#endif
int indexA = pc.indexA;
int indexB = pc.indexB;
@@ -693,25 +875,22 @@ namespace FarseerPhysics.Dynamics.Contacts
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);
Transform xfA = new Transform(Vector2.Zero, aA);
Transform xfB = new Transform(Vector2.Zero, aB);
xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
Vector2 normal;
Vector2 point;
float separation;
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation);
Vector2 rA = point - cA;
Vector2 rB = point - cB;
@@ -723,8 +902,8 @@ namespace FarseerPhysics.Dynamics.Contacts
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 rnA = MathUtils.Cross(ref rA, ref normal);
float rnB = MathUtils.Cross(ref rB, ref normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
@@ -733,17 +912,22 @@ namespace FarseerPhysics.Dynamics.Contacts
Vector2 P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
aA -= iA * MathUtils.Cross(ref rA, ref P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
aB += iB * MathUtils.Cross(ref rB, ref P);
}
_positions[indexA].c = cA;
_positions[indexA].a = aA;
_positions[indexB].c = cB;
_positions[indexB].a = aB;
#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
// Unlock bodies.
System.Threading.Interlocked.Exchange(ref _positions[orderedIndexB].Lock, 0);
System.Threading.Interlocked.Exchange(ref _positions[orderedIndexA].Lock, 0);
#endif
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
@@ -791,18 +975,16 @@ namespace FarseerPhysics.Dynamics.Contacts
// 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);
Transform xfA = new Transform(Vector2.Zero, aA);
Transform xfB = new Transform(Vector2.Zero, aB);
xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
Vector2 normal;
Vector2 point;
float separation;
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation);
Vector2 rA = point - cA;
Vector2 rB = point - cB;
@@ -814,8 +996,8 @@ namespace FarseerPhysics.Dynamics.Contacts
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 rnA = MathUtils.Cross(ref rA, ref normal);
float rnB = MathUtils.Cross(ref rB, ref normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
@@ -824,10 +1006,10 @@ namespace FarseerPhysics.Dynamics.Contacts
Vector2 P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
aA -= iA * MathUtils.Cross(ref rA, ref P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
aB += iB * MathUtils.Cross(ref rB, ref P);
}
_positions[indexA].c = cA;
@@ -872,8 +1054,8 @@ namespace FarseerPhysics.Dynamics.Contacts
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);
Vector2 pointA = Transform.Multiply(ref manifold.LocalPoint, ref xfA);
Vector2 pointB = Transform.Multiply(manifold.Points[0].LocalPoint, ref xfB);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
normal = pointB - pointA;
@@ -888,12 +1070,12 @@ namespace FarseerPhysics.Dynamics.Contacts
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
normal = Complex.Multiply(ref manifold.LocalNormal, ref xfA.q);
Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfA);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfB);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
@@ -903,12 +1085,12 @@ namespace FarseerPhysics.Dynamics.Contacts
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
normal = Complex.Multiply(ref manifold.LocalNormal, ref xfB.q);
Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfB);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfA);
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
@@ -924,19 +1106,22 @@ namespace FarseerPhysics.Dynamics.Contacts
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)
public static void Initialize(ContactPositionConstraint pc, ref Transform xfA, ref 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]);
Vector2 pointA = Transform.Multiply(ref pc.localPoint, ref xfA);
Vector2 pointB = Transform.Multiply(pc.localPoints[0], ref xfB);
normal = pointB - pointA;
normal.Normalize();
// Handle zero normalization
if (normal != Vector2.Zero)
normal.Normalize();
point = 0.5f * (pointA + pointB);
separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
}
@@ -944,10 +1129,10 @@ namespace FarseerPhysics.Dynamics.Contacts
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, pc.localNormal);
Vector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);
Complex.Multiply(ref pc.localNormal, ref xfA.q, out normal);
Vector2 planePoint = Transform.Multiply(ref pc.localPoint, ref xfA);
Vector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
Vector2 clipPoint = Transform.Multiply(pc.localPoints[index], ref xfB);
separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
}
@@ -955,10 +1140,10 @@ namespace FarseerPhysics.Dynamics.Contacts
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, pc.localNormal);
Vector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);
Complex.Multiply(ref pc.localNormal, ref xfB.q, out normal);
Vector2 planePoint = Transform.Multiply(ref pc.localPoint, ref xfB);
Vector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
Vector2 clipPoint = Transform.Multiply(pc.localPoints[index], ref xfA);
separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
@@ -976,4 +1161,6 @@ namespace FarseerPhysics.Dynamics.Contacts
}
}
}
}