Skip to content

Commit 6363605

Browse files
ikpilikpil
committed
wip
1 parent 7c1fc4d commit 6363605

6 files changed

Lines changed: 292 additions & 188 deletions

File tree

‎src/Box2D.NET.Samples/Samples/Collisions/DynamicTree.cs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -139,7 +139,7 @@ void BuildTree()
139139

140140
float y = -4.0f;
141141

142-
m_tree = b2DynamicTree_Create();
142+
m_tree = b2DynamicTree_Create(16);
143143

144144
B2Vec2 aabbMargin = new B2Vec2(0.1f, 0.1f);
145145

‎src/Box2D.NET/B2ContactSolvers.cs‎

Lines changed: 153 additions & 121 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
using static Box2D.NET.B2ConstraintGraphs;
1515
using static Box2D.NET.B2Bodies;
1616
using static Box2D.NET.B2Solvers;
17+
using static Box2D.NET.B2BitSets;
1718

1819
namespace Box2D.NET
1920
{
@@ -348,51 +349,54 @@ internal static void b2SolveOverflowContacts(B2StepContext context, bool useBias
348349
wB += iB * b2Cross(rB, P);
349350
}
350351

351-
// Friction
352-
for (int j = 0; j < pointCount; ++j)
352+
if (useBias == false)
353353
{
354-
ref B2ContactConstraintPoint cp = ref constraint.points[j];
354+
// Friction
355+
for (int j = 0; j < pointCount; ++j)
356+
{
357+
ref B2ContactConstraintPoint cp = ref constraint.points[j];
355358

356-
// fixed anchor points
357-
B2Vec2 rA = cp.anchorA;
358-
B2Vec2 rB = cp.anchorB;
359+
// fixed anchor points
360+
B2Vec2 rA = cp.anchorA;
361+
B2Vec2 rB = cp.anchorB;
359362

360-
// relative tangent velocity at contact
361-
B2Vec2 vrB = b2Add(vB, b2CrossSV(wB, rB));
362-
B2Vec2 vrA = b2Add(vA, b2CrossSV(wA, rA));
363+
// relative tangent velocity at contact
364+
B2Vec2 vrB = b2Add(vB, b2CrossSV(wB, rB));
365+
B2Vec2 vrA = b2Add(vA, b2CrossSV(wA, rA));
363366

364-
// vt = dot(vrB - sB * tangent - (vrA + sA * tangent), tangent)
365-
// = dot(vrB - vrA, tangent) - (sA + sB)
367+
// vt = dot(vrB - sB * tangent - (vrA + sA * tangent), tangent)
368+
// = dot(vrB - vrA, tangent) - (sA + sB)
366369

367-
float vt = b2Dot(b2Sub(vrB, vrA), tangent) - constraint.tangentSpeed;
370+
float vt = b2Dot(b2Sub(vrB, vrA), tangent) - constraint.tangentSpeed;
368371

369-
// incremental tangent impulse
370-
float impulse = cp.tangentMass * (-vt);
372+
// incremental tangent impulse
373+
float impulse = cp.tangentMass * (-vt);
371374

372-
// clamp the accumulated force
373-
float maxFriction = friction * cp.normalImpulse;
374-
float newImpulse = b2ClampFloat(cp.tangentImpulse + impulse, -maxFriction, maxFriction);
375-
impulse = newImpulse - cp.tangentImpulse;
376-
cp.tangentImpulse = newImpulse;
375+
// clamp the accumulated force
376+
float maxFriction = friction * cp.normalImpulse;
377+
float newImpulse = b2ClampFloat(cp.tangentImpulse + impulse, -maxFriction, maxFriction);
378+
impulse = newImpulse - cp.tangentImpulse;
379+
cp.tangentImpulse = newImpulse;
377380

378-
// apply tangent impulse
379-
B2Vec2 P = b2MulSV(impulse, tangent);
380-
vA = b2MulSub(vA, mA, P);
381-
wA -= iA * b2Cross(rA, P);
382-
vB = b2MulAdd(vB, mB, P);
383-
wB += iB * b2Cross(rB, P);
384-
}
381+
// apply tangent impulse
382+
B2Vec2 P = b2MulSV(impulse, tangent);
383+
vA = b2MulSub(vA, mA, P);
384+
wA -= iA * b2Cross(rA, P);
385+
vB = b2MulAdd(vB, mB, P);
386+
wB += iB * b2Cross(rB, P);
387+
}
385388

386-
// Rolling resistance
387-
{
388-
float deltaLambda = -constraint.rollingMass * (wB - wA);
389-
float lambda = constraint.rollingImpulse;
390-
float maxLambda = constraint.rollingResistance * totalNormalImpulse;
391-
constraint.rollingImpulse = b2ClampFloat(lambda + deltaLambda, -maxLambda, maxLambda);
392-
deltaLambda = constraint.rollingImpulse - lambda;
393-
394-
wA -= iA * deltaLambda;
395-
wB += iB * deltaLambda;
389+
// Rolling resistance
390+
{
391+
float deltaLambda = -constraint.rollingMass * (wB - wA);
392+
float lambda = constraint.rollingImpulse;
393+
float maxLambda = constraint.rollingResistance * totalNormalImpulse;
394+
constraint.rollingImpulse = b2ClampFloat(lambda + deltaLambda, -maxLambda, maxLambda);
395+
deltaLambda = constraint.rollingImpulse - lambda;
396+
397+
wA -= iA * deltaLambda;
398+
wB += iB * deltaLambda;
399+
}
396400
}
397401

398402
if (0 != (stateA.flags & (uint)B2BodyFlags.b2_dynamicFlag))
@@ -1707,93 +1711,96 @@ internal static void b2SolveContactsTask(int startIndex, int endIndex, B2StepCon
17071711
bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px)));
17081712
}
17091713

1710-
B2FloatW tangentX = c.normal.Y;
1711-
B2FloatW tangentY = b2SubW(b2ZeroW(), c.normal.X);
1712-
1713-
// point 1 friction constraint
1714-
{
1715-
// fixed anchors for Jacobians
1716-
B2Vec2W rA = c.anchorA1;
1717-
B2Vec2W rB = c.anchorB1;
1718-
1719-
// Relative velocity at contact
1720-
B2FloatW dvx = b2SubW(b2SubW(bB.v.X, b2MulW(bB.w, rB.Y)), b2SubW(bA.v.X, b2MulW(bA.w, rA.Y)));
1721-
B2FloatW dvy = b2SubW(b2AddW(bB.v.Y, b2MulW(bB.w, rB.X)), b2AddW(bA.v.Y, b2MulW(bA.w, rA.X)));
1722-
B2FloatW vt = b2AddW(b2MulW(dvx, tangentX), b2MulW(dvy, tangentY));
1723-
1724-
// Tangent speed (conveyor belt)
1725-
vt = b2SubW(vt, c.tangentSpeed);
1726-
1727-
// Compute tangent force
1728-
B2FloatW negImpulse = b2MulW(c.tangentMass1, vt);
1729-
1730-
// Clamp the accumulated force
1731-
B2FloatW maxFriction = b2MulW(c.friction, c.normalImpulse1);
1732-
B2FloatW newImpulse = b2SubW(c.tangentImpulse1, negImpulse);
1733-
newImpulse = b2MaxW(b2SubW(b2ZeroW(), maxFriction), b2MinW(newImpulse, maxFriction));
1734-
B2FloatW impulse = b2SubW(newImpulse, c.tangentImpulse1);
1735-
c.tangentImpulse1 = newImpulse;
1736-
1737-
// Apply contact impulse
1738-
B2FloatW Px = b2MulW(impulse, tangentX);
1739-
B2FloatW Py = b2MulW(impulse, tangentY);
1740-
1741-
bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px);
1742-
bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py);
1743-
bA.w = b2MulSubW(bA.w, c.invIA, b2SubW(b2MulW(rA.X, Py), b2MulW(rA.Y, Px)));
1744-
1745-
bB.v.X = b2MulAddW(bB.v.X, c.invMassB, Px);
1746-
bB.v.Y = b2MulAddW(bB.v.Y, c.invMassB, Py);
1747-
bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px)));
1748-
}
1749-
1750-
// second point friction constraint
1714+
if (useBias == false)
17511715
{
1752-
// fixed anchors for Jacobians
1753-
B2Vec2W rA = c.anchorA2;
1754-
B2Vec2W rB = c.anchorB2;
1716+
B2FloatW tangentX = c.normal.Y;
1717+
B2FloatW tangentY = b2SubW(b2ZeroW(), c.normal.X);
17551718

1756-
// Relative velocity at contact
1757-
B2FloatW dvx = b2SubW(b2SubW(bB.v.X, b2MulW(bB.w, rB.Y)), b2SubW(bA.v.X, b2MulW(bA.w, rA.Y)));
1758-
B2FloatW dvy = b2SubW(b2AddW(bB.v.Y, b2MulW(bB.w, rB.X)), b2AddW(bA.v.Y, b2MulW(bA.w, rA.X)));
1759-
B2FloatW vt = b2AddW(b2MulW(dvx, tangentX), b2MulW(dvy, tangentY));
1760-
1761-
// Tangent speed (conveyor belt)
1762-
vt = b2SubW(vt, c.tangentSpeed);
1763-
1764-
// Compute tangent force
1765-
B2FloatW negImpulse = b2MulW(c.tangentMass2, vt);
1766-
1767-
// Clamp the accumulated force
1768-
B2FloatW maxFriction = b2MulW(c.friction, c.normalImpulse2);
1769-
B2FloatW newImpulse = b2SubW(c.tangentImpulse2, negImpulse);
1770-
newImpulse = b2MaxW(b2SubW(b2ZeroW(), maxFriction), b2MinW(newImpulse, maxFriction));
1771-
B2FloatW impulse = b2SubW(newImpulse, c.tangentImpulse2);
1772-
c.tangentImpulse2 = newImpulse;
1773-
1774-
// Apply contact impulse
1775-
B2FloatW Px = b2MulW(impulse, tangentX);
1776-
B2FloatW Py = b2MulW(impulse, tangentY);
1777-
1778-
bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px);
1779-
bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py);
1780-
bA.w = b2MulSubW(bA.w, c.invIA, b2SubW(b2MulW(rA.X, Py), b2MulW(rA.Y, Px)));
1719+
// point 1 friction constraint
1720+
{
1721+
// Fixed anchor points for applying impulses
1722+
B2Vec2W rA = c.anchorA1;
1723+
B2Vec2W rB = c.anchorB1;
1724+
1725+
// Relative velocity at contact
1726+
B2FloatW dvx = b2SubW(b2SubW(bB.v.X, b2MulW(bB.w, rB.Y)), b2SubW(bA.v.X, b2MulW(bA.w, rA.Y)));
1727+
B2FloatW dvy = b2SubW(b2AddW(bB.v.Y, b2MulW(bB.w, rB.X)), b2AddW(bA.v.Y, b2MulW(bA.w, rA.X)));
1728+
B2FloatW vt = b2AddW(b2MulW(dvx, tangentX), b2MulW(dvy, tangentY));
1729+
1730+
// Tangent speed (conveyor belt)
1731+
vt = b2SubW(vt, c.tangentSpeed);
1732+
1733+
// Compute tangent force
1734+
B2FloatW negImpulse = b2MulW(c.tangentMass1, vt);
1735+
1736+
// Clamp the accumulated force
1737+
B2FloatW maxFriction = b2MulW(c.friction, c.normalImpulse1);
1738+
B2FloatW newImpulse = b2SubW(c.tangentImpulse1, negImpulse);
1739+
newImpulse = b2MaxW(b2SubW(b2ZeroW(), maxFriction), b2MinW(newImpulse, maxFriction));
1740+
B2FloatW impulse = b2SubW(newImpulse, c.tangentImpulse1);
1741+
c.tangentImpulse1 = newImpulse;
1742+
1743+
// Apply contact impulse
1744+
B2FloatW Px = b2MulW(impulse, tangentX);
1745+
B2FloatW Py = b2MulW(impulse, tangentY);
1746+
1747+
bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px);
1748+
bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py);
1749+
bA.w = b2MulSubW(bA.w, c.invIA, b2SubW(b2MulW(rA.X, Py), b2MulW(rA.Y, Px)));
1750+
1751+
bB.v.X = b2MulAddW(bB.v.X, c.invMassB, Px);
1752+
bB.v.Y = b2MulAddW(bB.v.Y, c.invMassB, Py);
1753+
bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px)));
1754+
}
17811755

1782-
bB.v.X = b2MulAddW(bB.v.X, c.invMassB, Px);
1783-
bB.v.Y = b2MulAddW(bB.v.Y, c.invMassB, Py);
1784-
bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px)));
1785-
}
1756+
// second point friction constraint
1757+
{
1758+
// fixed anchors for Jacobians
1759+
B2Vec2W rA = c.anchorA2;
1760+
B2Vec2W rB = c.anchorB2;
1761+
1762+
// Relative velocity at contact
1763+
B2FloatW dvx = b2SubW(b2SubW(bB.v.X, b2MulW(bB.w, rB.Y)), b2SubW(bA.v.X, b2MulW(bA.w, rA.Y)));
1764+
B2FloatW dvy = b2SubW(b2AddW(bB.v.Y, b2MulW(bB.w, rB.X)), b2AddW(bA.v.Y, b2MulW(bA.w, rA.X)));
1765+
B2FloatW vt = b2AddW(b2MulW(dvx, tangentX), b2MulW(dvy, tangentY));
1766+
1767+
// Tangent speed (conveyor belt)
1768+
vt = b2SubW(vt, c.tangentSpeed);
1769+
1770+
// Compute tangent force
1771+
B2FloatW negImpulse = b2MulW(c.tangentMass2, vt);
1772+
1773+
// Clamp the accumulated force
1774+
B2FloatW maxFriction = b2MulW(c.friction, c.normalImpulse2);
1775+
B2FloatW newImpulse = b2SubW(c.tangentImpulse2, negImpulse);
1776+
newImpulse = b2MaxW(b2SubW(b2ZeroW(), maxFriction), b2MinW(newImpulse, maxFriction));
1777+
B2FloatW impulse = b2SubW(newImpulse, c.tangentImpulse2);
1778+
c.tangentImpulse2 = newImpulse;
1779+
1780+
// Apply contact impulse
1781+
B2FloatW Px = b2MulW(impulse, tangentX);
1782+
B2FloatW Py = b2MulW(impulse, tangentY);
1783+
1784+
bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px);
1785+
bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py);
1786+
bA.w = b2MulSubW(bA.w, c.invIA, b2SubW(b2MulW(rA.X, Py), b2MulW(rA.Y, Px)));
1787+
1788+
bB.v.X = b2MulAddW(bB.v.X, c.invMassB, Px);
1789+
bB.v.Y = b2MulAddW(bB.v.Y, c.invMassB, Py);
1790+
bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px)));
1791+
}
17861792

1787-
// Rolling resistance
1788-
{
1789-
B2FloatW deltaLambda = b2MulW(c.rollingMass, b2SubW(bA.w, bB.w));
1790-
B2FloatW lambda = c.rollingImpulse;
1791-
B2FloatW maxLambda = b2MulW(c.rollingResistance, totalNormalImpulse);
1792-
c.rollingImpulse = b2SymClampW(b2AddW(lambda, deltaLambda), maxLambda);
1793-
deltaLambda = b2SubW(c.rollingImpulse, lambda);
1794-
1795-
bA.w = b2MulSubW(bA.w, c.invIA, deltaLambda);
1796-
bB.w = b2MulAddW(bB.w, c.invIB, deltaLambda);
1793+
// Rolling resistance
1794+
{
1795+
B2FloatW deltaLambda = b2MulW(c.rollingMass, b2SubW(bA.w, bB.w));
1796+
B2FloatW lambda = c.rollingImpulse;
1797+
B2FloatW maxLambda = b2MulW(c.rollingResistance, totalNormalImpulse);
1798+
c.rollingImpulse = b2SymClampW(b2AddW(lambda, deltaLambda), maxLambda);
1799+
deltaLambda = b2SubW(c.rollingImpulse, lambda);
1800+
1801+
bA.w = b2MulSubW(bA.w, c.invIA, deltaLambda);
1802+
bB.w = b2MulAddW(bB.w, c.invIB, deltaLambda);
1803+
}
17971804
}
17981805

17991806
b2ScatterBodies(states, c.indexA.AsSpan(), ref bA);
@@ -1916,12 +1923,17 @@ internal static void b2ApplyRestitutionTask(int startIndex, int endIndex, B2Step
19161923
b2TracyCZoneEnd(B2TracyCZone.restitution);
19171924
}
19181925

1919-
internal static void b2StoreImpulsesTask(int startIndex, int endIndex, B2StepContext context)
1926+
internal static void b2StoreImpulsesTask(int startIndex, int endIndex, B2StepContext context, int workerIndex)
19201927
{
19211928
b2TracyCZoneNC(B2TracyCZone.store_impulses, "Store", B2HexColor.b2_colorFireBrick, true);
19221929

1930+
B2World world = context.world;
19231931
Span<B2ContactSim> contacts = context.contacts;
19241932
Span<B2ContactConstraintWide> constraints = context.wideContactConstraints;
1933+
B2TaskContext taskContext = world.taskContexts.data[workerIndex];
1934+
ref B2BitSet hitEventBitSet = ref taskContext.hitEventBitSet;
1935+
bool hasHitEvents = taskContext.hasHitEvents;
1936+
float negHitThreshold = -world.hitEventThreshold;
19251937

19261938
B2Manifold dummy = new B2Manifold();
19271939

@@ -1942,7 +1954,8 @@ internal static void b2StoreImpulsesTask(int startIndex, int endIndex, B2StepCon
19421954

19431955
for (int laneIndex = 0; laneIndex < B2_SIMD_WIDTH; ++laneIndex)
19441956
{
1945-
ref B2Manifold m = ref contacts[baseIndex + laneIndex] == null ? ref dummy : ref contacts[baseIndex + laneIndex].manifold;
1957+
B2ContactSim contactSim = contacts[baseIndex + laneIndex];
1958+
ref B2Manifold m = ref contactSim == null ? ref dummy : ref contactSim.manifold;
19461959
m.rollingImpulse = rollingImpulse[laneIndex];
19471960

19481961
m.points[0].normalImpulse = normalImpulse1[laneIndex];
@@ -1954,9 +1967,28 @@ internal static void b2StoreImpulsesTask(int startIndex, int endIndex, B2StepCon
19541967
m.points[1].tangentImpulse = tangentImpulse2[laneIndex];
19551968
m.points[1].totalNormalImpulse = totalNormalImpulse2[laneIndex];
19561969
m.points[1].normalVelocity = normalVelocity2[laneIndex];
1970+
1971+
// Check for hit events to speed up serial processing later in the step
1972+
if (contactSim != null && (contactSim.simFlags & (uint)B2ContactSimFlags.b2_simEnableHitEvent) != 0)
1973+
{
1974+
for (int k = 0; k < contactSim.manifold.pointCount; ++k)
1975+
{
1976+
ref B2ManifoldPoint mp = ref m.points[k];
1977+
1978+
// Need to check total impulse because the point may be speculative and not colliding
1979+
if (mp.normalVelocity < negHitThreshold && mp.totalNormalImpulse > 0.0f)
1980+
{
1981+
b2SetBit(ref hitEventBitSet, contactSim.contactId);
1982+
hasHitEvents = true;
1983+
break;
1984+
}
1985+
}
1986+
}
19571987
}
19581988
}
19591989

1990+
taskContext.hasHitEvents = hasHitEvents;
1991+
19601992
b2TracyCZoneEnd(B2TracyCZone.store_impulses);
19611993
}
19621994
}

0 commit comments

Comments
 (0)