mirror of
https://github.com/space-wizards/RobustToolbox.git
synced 2026-02-15 03:30:53 +01:00
Revert physics jobs (#4663)
This commit is contained in:
metalgearsloth
@@ -10,7 +10,6 @@ using Robust.Shared.Physics.Components;
|
||||
using Robust.Shared.Physics.Dynamics;
|
||||
using Robust.Shared.Physics.Dynamics.Contacts;
|
||||
using Robust.Shared.Physics.Dynamics.Joints;
|
||||
using Robust.Shared.Threading;
|
||||
using Robust.Shared.Utility;
|
||||
|
||||
namespace Robust.Shared.Physics.Systems;
|
||||
@@ -193,6 +192,9 @@ public abstract partial class SharedPhysicsSystem
|
||||
private float _velocityThreshold;
|
||||
private float _baumgarte;
|
||||
|
||||
private const int VelocityConstraintsPerThread = 16;
|
||||
private const int PositionConstraintsPerThread = 16;
|
||||
|
||||
#region Setup
|
||||
|
||||
private void InitializeIsland()
|
||||
@@ -597,9 +599,6 @@ public abstract partial class SharedPhysicsSystem
|
||||
|
||||
private void SolveIslands(EntityUid uid, PhysicsMapComponent component, List<IslandData> islands, float frameTime, float dtRatio, float invDt, bool prediction)
|
||||
{
|
||||
if (islands.Count == 0)
|
||||
return;
|
||||
|
||||
var iBegin = 0;
|
||||
var gravity = _gravity.GetGravity(uid);
|
||||
|
||||
@@ -656,21 +655,27 @@ public abstract partial class SharedPhysicsSystem
|
||||
sleepStatus[i] = false;
|
||||
}
|
||||
|
||||
var job = new SolveIslandJob()
|
||||
var options = new ParallelOptions()
|
||||
{
|
||||
Physics = this,
|
||||
Islands = actualIslands,
|
||||
Data = data,
|
||||
Gravity = gravity,
|
||||
Prediction = prediction,
|
||||
SolvedPositions = solvedPositions,
|
||||
SolvedAngles = solvedAngles,
|
||||
LinearVelocities = linearVelocities,
|
||||
AngularVelocities = angularVelocities,
|
||||
SleepStatus = sleepStatus,
|
||||
MaxDegreeOfParallelism = _parallel.ParallelProcessCount,
|
||||
};
|
||||
|
||||
_parallel.ProcessNow(job, actualIslands.Length);
|
||||
while (iBegin < actualIslands.Length)
|
||||
{
|
||||
ref var island = ref actualIslands[iBegin];
|
||||
|
||||
if (!InternalParallel(island))
|
||||
break;
|
||||
|
||||
SolveIsland(ref island, in data, options, gravity, prediction, solvedPositions, solvedAngles, linearVelocities, angularVelocities, sleepStatus);
|
||||
iBegin++;
|
||||
}
|
||||
|
||||
Parallel.For(iBegin, actualIslands.Length, options, i =>
|
||||
{
|
||||
ref var island = ref actualIslands[i];
|
||||
SolveIsland(ref island, in data, null, gravity, prediction, solvedPositions, solvedAngles, linearVelocities, angularVelocities, sleepStatus);
|
||||
});
|
||||
|
||||
// Update data sequentially
|
||||
var metaQuery = GetEntityQuery<MetaDataComponent>();
|
||||
@@ -714,10 +719,10 @@ public abstract partial class SharedPhysicsSystem
|
||||
/// <summary>
|
||||
/// Go through all the bodies in this island and solve.
|
||||
/// </summary>
|
||||
/// <param name="parallel">Should we run internal tasks in parallel (true), or is the entire island being solved in parallel with others (false).</param>
|
||||
private void SolveIsland(
|
||||
ref IslandData island,
|
||||
in SolverData data,
|
||||
ParallelOptions? options,
|
||||
Vector2 gravity,
|
||||
bool prediction,
|
||||
Vector2[] solvedPositions,
|
||||
@@ -817,7 +822,7 @@ public abstract partial class SharedPhysicsSystem
|
||||
island.BrokenJoints.Add((island.Joints[j].Original, error));
|
||||
}
|
||||
|
||||
SolveVelocityConstraints(island, velocityConstraints, linearVelocities, angularVelocities);
|
||||
SolveVelocityConstraints(island, options, velocityConstraints, linearVelocities, angularVelocities);
|
||||
}
|
||||
|
||||
// Store for warm starting.
|
||||
@@ -856,7 +861,7 @@ public abstract partial class SharedPhysicsSystem
|
||||
|
||||
for (var i = 0; i < data.PositionIterations; i++)
|
||||
{
|
||||
var contactsOkay = SolvePositionConstraints(data, in island, positionConstraints, positions, angles);
|
||||
var contactsOkay = SolvePositionConstraints(data, in island, options, positionConstraints, positions, angles);
|
||||
var jointsOkay = true;
|
||||
|
||||
for (var j = 0; j < island.Joints.Count; ++j)
|
||||
@@ -886,19 +891,23 @@ public abstract partial class SharedPhysicsSystem
|
||||
// Solve positions now and store for later; we can't write this safely in parallel.
|
||||
var bodies = island.Bodies;
|
||||
|
||||
var finaliseJob = new FinalisePositionJob()
|
||||
if (options != null)
|
||||
{
|
||||
Physics = this,
|
||||
Offset = offset,
|
||||
Bodies = bodies,
|
||||
XformQuery = xformQuery,
|
||||
Positions = positions,
|
||||
Angles = angles,
|
||||
SolvedPositions = solvedPositions,
|
||||
SolvedAngles = solvedAngles,
|
||||
};
|
||||
const int FinaliseBodies = 32;
|
||||
var batches = (int)MathF.Ceiling((float) bodyCount / FinaliseBodies);
|
||||
|
||||
_parallel.ProcessSerialNow(finaliseJob, bodyCount);
|
||||
Parallel.For(0, batches, options, i =>
|
||||
{
|
||||
var start = i * FinaliseBodies;
|
||||
var end = Math.Min(bodyCount, start + FinaliseBodies);
|
||||
|
||||
FinalisePositions(start, end, offset, bodies, xformQuery, positions, angles, solvedPositions, solvedAngles);
|
||||
});
|
||||
}
|
||||
else
|
||||
{
|
||||
FinalisePositions(0, bodyCount, offset, bodies,xformQuery, positions, angles, solvedPositions, solvedAngles);
|
||||
}
|
||||
|
||||
// Check sleep status for all of the bodies
|
||||
// Writing sleep timer is safe but updating awake or not is not safe.
|
||||
@@ -975,26 +984,29 @@ public abstract partial class SharedPhysicsSystem
|
||||
ArrayPool<ContactPositionConstraint>.Shared.Return(positionConstraints);
|
||||
}
|
||||
|
||||
private void FinalisePositions(int index, int offset, List<PhysicsComponent> bodies, EntityQuery<TransformComponent> xformQuery, Vector2[] positions, float[] angles, Vector2[] solvedPositions, float[] solvedAngles)
|
||||
private void FinalisePositions(int start, int end, int offset, List<PhysicsComponent> bodies, EntityQuery<TransformComponent> xformQuery, Vector2[] positions, float[] angles, Vector2[] solvedPositions, float[] solvedAngles)
|
||||
{
|
||||
var body = bodies[index];
|
||||
for (var i = start; i < end; i++)
|
||||
{
|
||||
var body = bodies[i];
|
||||
|
||||
if (body.BodyType == BodyType.Static)
|
||||
return;
|
||||
if (body.BodyType == BodyType.Static)
|
||||
continue;
|
||||
|
||||
var xform = xformQuery.GetComponent(body.Owner);
|
||||
var parentXform = xformQuery.GetComponent(xform.ParentUid);
|
||||
var (_, parentRot, parentInvMatrix) = parentXform.GetWorldPositionRotationInvMatrix(xformQuery);
|
||||
var worldRot = (float) (parentRot + xform._localRotation);
|
||||
var xform = xformQuery.GetComponent(body.Owner);
|
||||
var parentXform = xformQuery.GetComponent(xform.ParentUid);
|
||||
var (_, parentRot, parentInvMatrix) = parentXform.GetWorldPositionRotationInvMatrix(xformQuery);
|
||||
var worldRot = (float) (parentRot + xform._localRotation);
|
||||
|
||||
var angle = angles[index];
|
||||
var angle = angles[i];
|
||||
|
||||
var q = new Quaternion2D(angle);
|
||||
var adjustedPosition = positions[index] - Physics.Transform.Mul(q, body.LocalCenter);
|
||||
var q = new Quaternion2D(angle);
|
||||
var adjustedPosition = positions[i] - Physics.Transform.Mul(q, body.LocalCenter);
|
||||
|
||||
var solvedPosition = parentInvMatrix.Transform(adjustedPosition);
|
||||
solvedPositions[offset + index] = solvedPosition - xform.LocalPosition;
|
||||
solvedAngles[offset + index] = angles[index] - worldRot;
|
||||
var solvedPosition = parentInvMatrix.Transform(adjustedPosition);
|
||||
solvedPositions[offset + i] = solvedPosition - xform.LocalPosition;
|
||||
solvedAngles[offset + i] = angles[i] - worldRot;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -1055,7 +1067,7 @@ public abstract partial class SharedPhysicsSystem
|
||||
}
|
||||
|
||||
// TODO: Should check if the values update.
|
||||
Dirty(uid, body, metaQuery.GetComponent(uid));
|
||||
Dirty(body, metaQuery.GetComponent(uid));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1075,49 +1087,4 @@ public abstract partial class SharedPhysicsSystem
|
||||
SetAwake(body.Owner, body, false);
|
||||
}
|
||||
}
|
||||
|
||||
#region Jobs
|
||||
|
||||
private record struct SolveIslandJob : IParallelRobustJob
|
||||
{
|
||||
public int BatchSize => 1;
|
||||
|
||||
public SharedPhysicsSystem Physics;
|
||||
public IslandData[] Islands;
|
||||
public SolverData Data;
|
||||
public Vector2 Gravity;
|
||||
public bool Prediction;
|
||||
public Vector2[] SolvedPositions;
|
||||
public float[] SolvedAngles;
|
||||
public Vector2[] LinearVelocities;
|
||||
public float[] AngularVelocities;
|
||||
public bool[] SleepStatus;
|
||||
|
||||
public void Execute(int index)
|
||||
{
|
||||
ref var island = ref Islands[index];
|
||||
Physics.SolveIsland(ref island, Data,Gravity, Prediction, SolvedPositions, SolvedAngles, LinearVelocities, AngularVelocities, SleepStatus);
|
||||
}
|
||||
}
|
||||
|
||||
private record struct FinalisePositionJob : IParallelRobustJob
|
||||
{
|
||||
public int BatchSize => 32;
|
||||
|
||||
public SharedPhysicsSystem Physics;
|
||||
public EntityQuery<TransformComponent> XformQuery;
|
||||
public int Offset;
|
||||
public List<PhysicsComponent> Bodies;
|
||||
public Vector2[] Positions;
|
||||
public float[] Angles;
|
||||
public Vector2[] SolvedPositions;
|
||||
public float[] SolvedAngles;
|
||||
|
||||
public void Execute(int index)
|
||||
{
|
||||
Physics.FinalisePositions(index, Offset, Bodies, XformQuery, Positions, Angles, SolvedPositions, SolvedAngles);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
|
||||
@@ -21,7 +21,6 @@
|
||||
*/
|
||||
|
||||
using System;
|
||||
using System.Buffers;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
@@ -30,7 +29,6 @@ using Robust.Shared.Physics.Collision;
|
||||
using Robust.Shared.Physics.Components;
|
||||
using Robust.Shared.Physics.Dynamics;
|
||||
using Robust.Shared.Physics.Dynamics.Contacts;
|
||||
using Robust.Shared.Threading;
|
||||
using Robust.Shared.Utility;
|
||||
|
||||
namespace Robust.Shared.Physics.Systems;
|
||||
@@ -296,16 +294,11 @@ public abstract partial class SharedPhysicsSystem
|
||||
Vector2[] linearVelocities,
|
||||
float[] angularVelocities)
|
||||
{
|
||||
var contactCount = island.Contacts.Count;
|
||||
|
||||
if (contactCount == 0)
|
||||
return;
|
||||
|
||||
var offset = island.Offset;
|
||||
|
||||
for (var i = 0; i < contactCount; ++i)
|
||||
for (var i = 0; i < island.Contacts.Count; ++i)
|
||||
{
|
||||
ref var velocityConstraint = ref velocityConstraints[i];
|
||||
var velocityConstraint = velocityConstraints[i];
|
||||
|
||||
var indexA = velocityConstraint.IndexA;
|
||||
var indexB = velocityConstraint.IndexB;
|
||||
@@ -336,302 +329,312 @@ public abstract partial class SharedPhysicsSystem
|
||||
}
|
||||
|
||||
private void SolveVelocityConstraints(IslandData island,
|
||||
ParallelOptions? options,
|
||||
ContactVelocityConstraint[] velocityConstraints,
|
||||
Vector2[] linearVelocities,
|
||||
float[] angularVelocities)
|
||||
{
|
||||
var contactCount = island.Contacts.Count;
|
||||
|
||||
if (contactCount == 0)
|
||||
return;
|
||||
|
||||
var job = new SolveVelocityJob()
|
||||
if (options != null && contactCount > VelocityConstraintsPerThread * 2)
|
||||
{
|
||||
Physics = this,
|
||||
Island = island,
|
||||
VelocityConstraints = velocityConstraints,
|
||||
LinearVelocities = linearVelocities,
|
||||
AngularVelocities = angularVelocities,
|
||||
};
|
||||
var batches = (int) Math.Ceiling((float) contactCount / VelocityConstraintsPerThread);
|
||||
|
||||
_parallel.ProcessSerialNow(job, contactCount);
|
||||
Parallel.For(0, batches, options, i =>
|
||||
{
|
||||
var start = i * VelocityConstraintsPerThread;
|
||||
var end = Math.Min(start + VelocityConstraintsPerThread, contactCount);
|
||||
SolveVelocityConstraints(island, start, end, velocityConstraints, linearVelocities, angularVelocities);
|
||||
});
|
||||
}
|
||||
else
|
||||
{
|
||||
SolveVelocityConstraints(island, 0, contactCount, velocityConstraints, linearVelocities, angularVelocities);
|
||||
}
|
||||
}
|
||||
|
||||
private void SolveVelocityConstraint(
|
||||
private void SolveVelocityConstraints(
|
||||
IslandData island,
|
||||
ref ContactVelocityConstraint velocityConstraint,
|
||||
int start,
|
||||
int end,
|
||||
ContactVelocityConstraint[] velocityConstraints,
|
||||
Vector2[] linearVelocities,
|
||||
float[] angularVelocities)
|
||||
{
|
||||
var offset = island.Offset;
|
||||
|
||||
// Here be dragons
|
||||
var indexA = velocityConstraint.IndexA;
|
||||
var indexB = velocityConstraint.IndexB;
|
||||
var mA = velocityConstraint.InvMassA;
|
||||
var iA = velocityConstraint.InvIA;
|
||||
var mB = velocityConstraint.InvMassB;
|
||||
var iB = velocityConstraint.InvIB;
|
||||
var pointCount = velocityConstraint.PointCount;
|
||||
|
||||
ref var vA = ref linearVelocities[offset + indexA];
|
||||
ref var wA = ref angularVelocities[offset + indexA];
|
||||
ref var vB = ref linearVelocities[offset + indexB];
|
||||
ref var wB = ref angularVelocities[offset + indexB];
|
||||
|
||||
var normal = velocityConstraint.Normal;
|
||||
var tangent = Vector2Helpers.Cross(normal, 1.0f);
|
||||
var friction = velocityConstraint.Friction;
|
||||
|
||||
DebugTools.Assert(pointCount is 1 or 2);
|
||||
|
||||
// Solve tangent constraints first because non-penetration is more important
|
||||
// than friction.
|
||||
for (var j = 0; j < pointCount; ++j)
|
||||
for (var i = start; i < end; ++i)
|
||||
{
|
||||
ref var velConstraintPoint = ref velocityConstraint.Points[j];
|
||||
ref var velocityConstraint = ref velocityConstraints[i];
|
||||
|
||||
// Relative velocity at contact
|
||||
var dv = vB + Vector2Helpers.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, velConstraintPoint.RelativeVelocityA);
|
||||
var indexA = velocityConstraint.IndexA;
|
||||
var indexB = velocityConstraint.IndexB;
|
||||
var mA = velocityConstraint.InvMassA;
|
||||
var iA = velocityConstraint.InvIA;
|
||||
var mB = velocityConstraint.InvMassB;
|
||||
var iB = velocityConstraint.InvIB;
|
||||
var pointCount = velocityConstraint.PointCount;
|
||||
|
||||
// Compute tangent force
|
||||
float vt = Vector2.Dot(dv, tangent) - velocityConstraint.TangentSpeed;
|
||||
float lambda = velConstraintPoint.TangentMass * (-vt);
|
||||
ref var vA = ref linearVelocities[offset + indexA];
|
||||
ref var wA = ref angularVelocities[offset + indexA];
|
||||
ref var vB = ref linearVelocities[offset + indexB];
|
||||
ref var wB = ref angularVelocities[offset + indexB];
|
||||
|
||||
// b2Clamp the accumulated force
|
||||
var maxFriction = friction * velConstraintPoint.NormalImpulse;
|
||||
var newImpulse = Math.Clamp(velConstraintPoint.TangentImpulse + lambda, -maxFriction, maxFriction);
|
||||
lambda = newImpulse - velConstraintPoint.TangentImpulse;
|
||||
velConstraintPoint.TangentImpulse = newImpulse;
|
||||
var normal = velocityConstraint.Normal;
|
||||
var tangent = Vector2Helpers.Cross(normal, 1.0f);
|
||||
var friction = velocityConstraint.Friction;
|
||||
|
||||
// Apply contact impulse
|
||||
Vector2 P = tangent * lambda;
|
||||
DebugTools.Assert(pointCount is 1 or 2);
|
||||
|
||||
vA -= P * mA;
|
||||
wA -= iA * Vector2Helpers.Cross(velConstraintPoint.RelativeVelocityA, P);
|
||||
|
||||
vB += P * mB;
|
||||
wB += iB * Vector2Helpers.Cross(velConstraintPoint.RelativeVelocityB, P);
|
||||
}
|
||||
|
||||
// Solve normal constraints
|
||||
if (velocityConstraint.PointCount == 1)
|
||||
{
|
||||
ref var vcp = ref velocityConstraint.Points[0];
|
||||
|
||||
// Relative velocity at contact
|
||||
Vector2 dv = vB + Vector2Helpers.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, vcp.RelativeVelocityA);
|
||||
|
||||
// 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 = normal * lambda;
|
||||
vA -= P * mA;
|
||||
wA -= iA * Vector2Helpers.Cross(vcp.RelativeVelocityA, P);
|
||||
|
||||
vB += P * mB;
|
||||
wB += iB * Vector2Helpers.Cross(vcp.RelativeVelocityB, 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;
|
||||
|
||||
ref var cp1 = ref velocityConstraint.Points[0];
|
||||
ref var cp2 = ref velocityConstraint.Points[1];
|
||||
|
||||
Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
|
||||
DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f);
|
||||
|
||||
// Relative velocity at contact
|
||||
Vector2 dv1 = vB + Vector2Helpers.Cross(wB, cp1.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, cp1.RelativeVelocityA);
|
||||
Vector2 dv2 = vB + Vector2Helpers.Cross(wB, cp2.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, cp2.RelativeVelocityA);
|
||||
|
||||
// Compute normal velocity
|
||||
float vn1 = Vector2.Dot(dv1, normal);
|
||||
float vn2 = Vector2.Dot(dv2, normal);
|
||||
|
||||
Vector2 b = new Vector2
|
||||
// Solve tangent constraints first because non-penetration is more important
|
||||
// than friction.
|
||||
for (var j = 0; j < pointCount; ++j)
|
||||
{
|
||||
X = vn1 - cp1.VelocityBias,
|
||||
Y = vn2 - cp2.VelocityBias
|
||||
};
|
||||
ref var velConstraintPoint = ref velocityConstraint.Points[j];
|
||||
|
||||
// Compute b'
|
||||
b -= Physics.Transform.Mul(velocityConstraint.K, a);
|
||||
// Relative velocity at contact
|
||||
var dv = vB + Vector2Helpers.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, velConstraintPoint.RelativeVelocityA);
|
||||
|
||||
//const float k_errorTol = 1e-3f;
|
||||
//B2_NOT_USED(k_errorTol);
|
||||
// Compute tangent force
|
||||
float vt = Vector2.Dot(dv, tangent) - velocityConstraint.TangentSpeed;
|
||||
float lambda = velConstraintPoint.TangentMass * (-vt);
|
||||
|
||||
for (; ; )
|
||||
// b2Clamp the accumulated force
|
||||
var maxFriction = friction * velConstraintPoint.NormalImpulse;
|
||||
var newImpulse = Math.Clamp(velConstraintPoint.TangentImpulse + lambda, -maxFriction, maxFriction);
|
||||
lambda = newImpulse - velConstraintPoint.TangentImpulse;
|
||||
velConstraintPoint.TangentImpulse = newImpulse;
|
||||
|
||||
// Apply contact impulse
|
||||
Vector2 P = tangent * lambda;
|
||||
|
||||
vA -= P * mA;
|
||||
wA -= iA * Vector2Helpers.Cross(velConstraintPoint.RelativeVelocityA, P);
|
||||
|
||||
vB += P * mB;
|
||||
wB += iB * Vector2Helpers.Cross(velConstraintPoint.RelativeVelocityB, P);
|
||||
}
|
||||
|
||||
// Solve normal constraints
|
||||
if (velocityConstraint.PointCount == 1)
|
||||
{
|
||||
//
|
||||
// Case 1: vn = 0
|
||||
//
|
||||
// 0 = A * x + b'
|
||||
//
|
||||
// Solve for x:
|
||||
//
|
||||
// x = - inv(A) * b'
|
||||
//
|
||||
Vector2 x = -Physics.Transform.Mul(velocityConstraint.NormalMass, b);
|
||||
ref var vcp = ref velocityConstraint.Points[0];
|
||||
|
||||
if (x.X >= 0.0f && x.Y >= 0.0f)
|
||||
// Relative velocity at contact
|
||||
Vector2 dv = vB + Vector2Helpers.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, vcp.RelativeVelocityA);
|
||||
|
||||
// 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 = normal * lambda;
|
||||
vA -= P * mA;
|
||||
wA -= iA * Vector2Helpers.Cross(vcp.RelativeVelocityA, P);
|
||||
|
||||
vB += P * mB;
|
||||
wB += iB * Vector2Helpers.Cross(vcp.RelativeVelocityB, 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;
|
||||
|
||||
ref var cp1 = ref velocityConstraint.Points[0];
|
||||
ref var cp2 = ref velocityConstraint.Points[1];
|
||||
|
||||
Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
|
||||
DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f);
|
||||
|
||||
// Relative velocity at contact
|
||||
Vector2 dv1 = vB + Vector2Helpers.Cross(wB, cp1.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, cp1.RelativeVelocityA);
|
||||
Vector2 dv2 = vB + Vector2Helpers.Cross(wB, cp2.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, cp2.RelativeVelocityA);
|
||||
|
||||
// Compute normal velocity
|
||||
float vn1 = Vector2.Dot(dv1, normal);
|
||||
float vn2 = Vector2.Dot(dv2, normal);
|
||||
|
||||
Vector2 b = new Vector2
|
||||
{
|
||||
// Get the incremental impulse
|
||||
Vector2 d = x - a;
|
||||
X = vn1 - cp1.VelocityBias,
|
||||
Y = vn2 - cp2.VelocityBias
|
||||
};
|
||||
|
||||
// Apply incremental impulse
|
||||
Vector2 P1 = normal * d.X;
|
||||
Vector2 P2 = normal * d.Y;
|
||||
vA -= (P1 + P2) * mA;
|
||||
wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
|
||||
// Compute b'
|
||||
b -= Physics.Transform.Mul(velocityConstraint.K, a);
|
||||
|
||||
vB += (P1 + P2) * mB;
|
||||
wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
|
||||
//const float k_errorTol = 1e-3f;
|
||||
//B2_NOT_USED(k_errorTol);
|
||||
|
||||
// Accumulate
|
||||
cp1.NormalImpulse = x.X;
|
||||
cp2.NormalImpulse = x.Y;
|
||||
for (; ; )
|
||||
{
|
||||
//
|
||||
// Case 1: vn = 0
|
||||
//
|
||||
// 0 = A * x + b'
|
||||
//
|
||||
// Solve for x:
|
||||
//
|
||||
// x = - inv(A) * b'
|
||||
//
|
||||
Vector2 x = -Physics.Transform.Mul(velocityConstraint.NormalMass, b);
|
||||
|
||||
if (x.X >= 0.0f && x.Y >= 0.0f)
|
||||
{
|
||||
// Get the incremental impulse
|
||||
Vector2 d = x - a;
|
||||
|
||||
// Apply incremental impulse
|
||||
Vector2 P1 = normal * d.X;
|
||||
Vector2 P2 = normal * d.Y;
|
||||
vA -= (P1 + P2) * mA;
|
||||
wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
|
||||
|
||||
vB += (P1 + P2) * mB;
|
||||
wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.NormalImpulse = x.X;
|
||||
cp2.NormalImpulse = x.Y;
|
||||
|
||||
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 = velocityConstraint.K.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 = normal * d.X;
|
||||
Vector2 P2 = normal * d.Y;
|
||||
vA -= (P1 + P2) * mA;
|
||||
wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
|
||||
|
||||
vB += (P1 + P2) * mB;
|
||||
wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.NormalImpulse = x.X;
|
||||
cp2.NormalImpulse = x.Y;
|
||||
|
||||
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 = velocityConstraint.K.Z * 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 = normal * d.X;
|
||||
Vector2 P2 = normal * d.Y;
|
||||
vA -= (P1 + P2) * mA;
|
||||
wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
|
||||
|
||||
vB += (P1 + P2) * mB;
|
||||
wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.NormalImpulse = x.X;
|
||||
cp2.NormalImpulse = x.Y;
|
||||
|
||||
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 = normal * d.X;
|
||||
Vector2 P2 = normal * d.Y;
|
||||
vA -= (P1 + P2) * mA;
|
||||
wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
|
||||
|
||||
vB += (P1 + P2) * mB;
|
||||
wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, 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;
|
||||
}
|
||||
|
||||
//
|
||||
// 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 = velocityConstraint.K.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 = normal * d.X;
|
||||
Vector2 P2 = normal * d.Y;
|
||||
vA -= (P1 + P2) * mA;
|
||||
wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
|
||||
|
||||
vB += (P1 + P2) * mB;
|
||||
wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.NormalImpulse = x.X;
|
||||
cp2.NormalImpulse = x.Y;
|
||||
|
||||
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 = velocityConstraint.K.Z * 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 = normal * d.X;
|
||||
Vector2 P2 = normal * d.Y;
|
||||
vA -= (P1 + P2) * mA;
|
||||
wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
|
||||
|
||||
vB += (P1 + P2) * mB;
|
||||
wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
|
||||
|
||||
// Accumulate
|
||||
cp1.NormalImpulse = x.X;
|
||||
cp2.NormalImpulse = x.Y;
|
||||
|
||||
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 = normal * d.X;
|
||||
Vector2 P2 = normal * d.Y;
|
||||
vA -= (P1 + P2) * mA;
|
||||
wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
|
||||
|
||||
vB += (P1 + P2) * mB;
|
||||
wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -655,110 +658,107 @@ public abstract partial class SharedPhysicsSystem
|
||||
private bool SolvePositionConstraints(
|
||||
SolverData data,
|
||||
in IslandData island,
|
||||
ParallelOptions? options,
|
||||
ContactPositionConstraint[] positionConstraints,
|
||||
Vector2[] positions,
|
||||
float[] angles)
|
||||
{
|
||||
var contactCount = island.Contacts.Count;
|
||||
|
||||
if (contactCount == 0)
|
||||
return true;
|
||||
|
||||
var solved = ArrayPool<bool>.Shared.Rent(contactCount);
|
||||
|
||||
var job = new SolvePositionJob()
|
||||
{
|
||||
Physics = this,
|
||||
Data = data,
|
||||
PositionConstraints = positionConstraints,
|
||||
Positions = positions,
|
||||
Angles = angles,
|
||||
Solved = solved
|
||||
};
|
||||
|
||||
// Parallel
|
||||
_parallel.ProcessSerialNow(job, contactCount);
|
||||
var isSolved = true;
|
||||
|
||||
for (var i = 0; i < contactCount; i++)
|
||||
if (options != null && contactCount > PositionConstraintsPerThread * 2)
|
||||
{
|
||||
if (solved[i])
|
||||
continue;
|
||||
var unsolved = 0;
|
||||
var batches = (int) Math.Ceiling((float) contactCount / PositionConstraintsPerThread);
|
||||
|
||||
isSolved = false;
|
||||
break;
|
||||
Parallel.For(0, batches, options, i =>
|
||||
{
|
||||
var start = i * PositionConstraintsPerThread;
|
||||
var end = Math.Min(start + PositionConstraintsPerThread, contactCount);
|
||||
|
||||
if (!SolvePositionConstraints(data, start, end, positionConstraints, positions, angles))
|
||||
Interlocked.Increment(ref unsolved);
|
||||
});
|
||||
|
||||
return unsolved == 0;
|
||||
}
|
||||
|
||||
ArrayPool<bool>.Shared.Return(solved);
|
||||
|
||||
return isSolved;
|
||||
// No parallel
|
||||
return SolvePositionConstraints(data, 0, contactCount, positionConstraints, positions, angles);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tries to solve positions for all contacts specified.
|
||||
/// </summary>
|
||||
/// <returns>true if all positions solved</returns>
|
||||
private bool SolvePositionConstraint(
|
||||
private bool SolvePositionConstraints(
|
||||
SolverData data,
|
||||
ref ContactPositionConstraint positionConstraint,
|
||||
int start,
|
||||
int end,
|
||||
ContactPositionConstraint[] positionConstraints,
|
||||
Vector2[] positions,
|
||||
float[] angles)
|
||||
{
|
||||
float minSeparation = 0.0f;
|
||||
|
||||
int indexA = positionConstraint.IndexA;
|
||||
int indexB = positionConstraint.IndexB;
|
||||
Vector2 localCenterA = positionConstraint.LocalCenterA;
|
||||
float mA = positionConstraint.InvMassA;
|
||||
float iA = positionConstraint.InvIA;
|
||||
Vector2 localCenterB = positionConstraint.LocalCenterB;
|
||||
float mB = positionConstraint.InvMassB;
|
||||
float iB = positionConstraint.InvIB;
|
||||
int pointCount = positionConstraint.PointCount;
|
||||
|
||||
ref var centerA = ref positions[indexA];
|
||||
ref var angleA = ref angles[indexA];
|
||||
ref var centerB = ref positions[indexB];
|
||||
ref var angleB = ref angles[indexB];
|
||||
|
||||
// Solve normal constraints
|
||||
for (int j = 0; j < pointCount; ++j)
|
||||
for (int i = start; i < end; ++i)
|
||||
{
|
||||
Transform xfA = new Transform(angleA);
|
||||
Transform xfB = new Transform(angleB);
|
||||
xfA.Position = centerA - Physics.Transform.Mul(xfA.Quaternion2D, localCenterA);
|
||||
xfB.Position = centerB - Physics.Transform.Mul(xfB.Quaternion2D, localCenterB);
|
||||
var pc = positionConstraints[i];
|
||||
|
||||
Vector2 normal;
|
||||
Vector2 point;
|
||||
float separation;
|
||||
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;
|
||||
|
||||
PositionSolverManifoldInitialize(positionConstraint, j, xfA, xfB, out normal, out point, out separation);
|
||||
ref var centerA = ref positions[indexA];
|
||||
ref var angleA = ref angles[indexA];
|
||||
ref var centerB = ref positions[indexB];
|
||||
ref var angleB = ref angles[indexB];
|
||||
|
||||
Vector2 rA = point - centerA;
|
||||
Vector2 rB = point - centerB;
|
||||
// Solve normal constraints
|
||||
for (int j = 0; j < pointCount; ++j)
|
||||
{
|
||||
Transform xfA = new Transform(angleA);
|
||||
Transform xfB = new Transform(angleB);
|
||||
xfA.Position = centerA - Physics.Transform.Mul(xfA.Quaternion2D, localCenterA);
|
||||
xfB.Position = centerB - Physics.Transform.Mul(xfB.Quaternion2D, localCenterB);
|
||||
|
||||
// Track max constraint error.
|
||||
minSeparation = Math.Min(minSeparation, separation);
|
||||
Vector2 normal;
|
||||
Vector2 point;
|
||||
float separation;
|
||||
|
||||
// Prevent large corrections and allow slop.
|
||||
float C = Math.Clamp(data.Baumgarte * (separation + PhysicsConstants.LinearSlop), -_maxLinearCorrection, 0.0f);
|
||||
PositionSolverManifoldInitialize(pc, j, xfA, xfB, out normal, out point, out separation);
|
||||
|
||||
// Compute the effective mass.
|
||||
float rnA = Vector2Helpers.Cross(rA, normal);
|
||||
float rnB = Vector2Helpers.Cross(rB, normal);
|
||||
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
||||
Vector2 rA = point - centerA;
|
||||
Vector2 rB = point - centerB;
|
||||
|
||||
// Compute normal impulse
|
||||
float impulse = K > 0.0f ? -C / K : 0.0f;
|
||||
// Track max constraint error.
|
||||
minSeparation = Math.Min(minSeparation, separation);
|
||||
|
||||
Vector2 P = normal * impulse;
|
||||
// Prevent large corrections and allow slop.
|
||||
float C = Math.Clamp(data.Baumgarte * (separation + PhysicsConstants.LinearSlop), -_maxLinearCorrection, 0.0f);
|
||||
|
||||
centerA -= P * mA;
|
||||
angleA -= iA * Vector2Helpers.Cross(rA, P);
|
||||
// Compute the effective mass.
|
||||
float rnA = Vector2Helpers.Cross(rA, normal);
|
||||
float rnB = Vector2Helpers.Cross(rB, normal);
|
||||
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
||||
|
||||
centerB += P * mB;
|
||||
angleB += iB * Vector2Helpers.Cross(rB, P);
|
||||
// Compute normal impulse
|
||||
float impulse = K > 0.0f ? -C / K : 0.0f;
|
||||
|
||||
Vector2 P = normal * impulse;
|
||||
|
||||
centerA -= P * mA;
|
||||
angleA -= iA * Vector2Helpers.Cross(rA, P);
|
||||
|
||||
centerB += P * mB;
|
||||
angleB += iB * Vector2Helpers.Cross(rB, P);
|
||||
}
|
||||
}
|
||||
|
||||
// We can't expect minSpeparation >= -b2_linearSlop because we don't
|
||||
@@ -907,51 +907,4 @@ public abstract partial class SharedPhysicsSystem
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#region Jobs
|
||||
|
||||
private record struct SolvePositionJob : IParallelRobustJob
|
||||
{
|
||||
public int BatchSize => 16;
|
||||
|
||||
public SharedPhysicsSystem Physics;
|
||||
public SolverData Data;
|
||||
public ContactPositionConstraint[] PositionConstraints;
|
||||
public Vector2[] Positions;
|
||||
public float[] Angles;
|
||||
public bool[] Solved;
|
||||
|
||||
public void Execute(int index)
|
||||
{
|
||||
ref var constraint = ref PositionConstraints[index];
|
||||
|
||||
if (Physics.SolvePositionConstraint(Data, ref constraint, Positions, Angles))
|
||||
{
|
||||
Solved[index] = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
Solved[index] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private record struct SolveVelocityJob : IParallelRobustJob
|
||||
{
|
||||
public int BatchSize => 16;
|
||||
|
||||
public SharedPhysicsSystem Physics;
|
||||
public IslandData Island;
|
||||
public ContactVelocityConstraint[] VelocityConstraints;
|
||||
public Vector2[] LinearVelocities;
|
||||
public float[] AngularVelocities;
|
||||
|
||||
public void Execute(int index)
|
||||
{
|
||||
ref var constraint = ref VelocityConstraints[index];
|
||||
Physics.SolveVelocityConstraint(Island, ref constraint, LinearVelocities, AngularVelocities);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user