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