public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA) { Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA); Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA); Vector3Wide.Add(wsvA.Angular, worldCorrectiveVelocityA, out wsvA.Angular); }
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TangentFriction.Projection data, ref Jacobians jacobians, ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) { Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear); Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular); Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Linear, jacobians.LinearA, out var csvbLinear); Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Angular, jacobians.AngularB, out var csvbAngular); //Note that the velocity in constraint space is (csvaLinear - csvbLinear + csvaAngular + csvbAngular). //The subtraction there is due to sharing the linear jacobian between both bodies3D. //In the following, we need to compute the constraint space *violating* velocity- which is the negation of the above velocity in constraint space. //So, (csvbLinear - csvaLinear - (csvaAngular + csvbAngular)). Vector2Wide.Subtract(csvbLinear, csvaLinear, out var csvLinear); Vector2Wide.Add(csvaAngular, csvbAngular, out var csvAngular); Vector2Wide.Subtract(csvLinear, csvAngular, out var csv); Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var csi); var previousAccumulated = accumulatedImpulse; Vector2Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse); //The maximum force of friction depends upon the normal impulse. The maximum is supplied per iteration. Vector2Wide.Length(accumulatedImpulse, out var accumulatedMagnitude); //Note division by zero guard. var scale = Vector.Min(Vector <float> .One, maximumImpulse / Vector.Max(new Vector <float>(1e-16f), accumulatedMagnitude)); Vector2Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse); Vector2Wide.Subtract(accumulatedImpulse, previousAccumulated, out correctiveCSI); }
public void WarmStart(ref BodyVelocities wsvA, ref ContactManifold1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z); TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); PenetrationLimit1OneBody.WarmStart(ref projection.Penetration, ref projection.InertiaA, ref projection.Normal, ref accumulatedImpulses.Penetration0, ref wsvA); }
public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertias inertiaA, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians); //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. //To compensate for this, the accumulated impulse should be scaled if dt changes. ApplyImpulse(ref jacobians, ref inertiaA, ref accumulatedImpulse, ref wsvA); }
public static void WarmStart( ref Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal, ref Vector <float> accumulatedImpulse0, ref Vector <float> accumulatedImpulse1, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { ApplyImpulse(ref projection.Penetration0, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse0, ref wsvA, ref wsvB); ApplyImpulse(ref projection.Penetration1, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse1, ref wsvA, ref wsvB); }
public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB) { ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse); //This function does not have a maximum impulse limit, so no clamping is required. Vector3Wide.Add(accumulatedImpulse, correctiveImpulse, out accumulatedImpulse); ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse); }
public static void Solve(ref Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal, ref Vector <float> accumulatedImpulse0, ref Vector <float> accumulatedImpulse1, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration0, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse0, out var correctiveCSI0); ApplyImpulse(ref projection.Penetration0, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI0, ref wsvA, ref wsvB); ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration1, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse1, out var correctiveCSI1); ApplyImpulse(ref projection.Penetration1, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI1, ref wsvA, ref wsvB); }
public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector <float> maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB) { ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse); //This function DOES have a maximum impulse limit. ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref correctiveImpulse); ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse); }
public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { Vector3Wide.Scale(ref angularJacobianA, ref correctiveImpulse, out var worldCorrectiveImpulseA); Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref worldCorrectiveImpulseA, ref inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA); Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref worldCorrectiveImpulseA, ref inertiaB.InverseInertiaTensor, out var worldCorrectiveVelocityB); Vector3Wide.Add(ref wsvA.AngularVelocity, ref worldCorrectiveVelocityA, out wsvA.AngularVelocity); Vector3Wide.Subtract(ref wsvB.AngularVelocity, ref worldCorrectiveVelocityB, out wsvB.AngularVelocity); }
static void TestScatter(Context context) { var a = new BodyVelocities(); var b = new BodyVelocities(); for (int i = 0; i < context.ConstraintBundleCount; ++i) { GatherScatter.ScatterVelocities(ref context.BodyVelocities, ref context.BodyReferences[i], Vector <float> .Count, ref a, ref b); } }
public void Solve(ref BodyVelocities wsvA, ref ContactManifold1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z); var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * accumulatedImpulses.Penetration0; TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. PenetrationLimit1OneBody.Solve(ref projection.Penetration, ref projection.InertiaA, ref projection.Normal, ref accumulatedImpulses.Penetration0, ref wsvA); }
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); BodyVelocities correctiveVelocityA; Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular); Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear); Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular); }
public void WarmStart(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z); TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); PenetrationLimit4.WarmStart(ref projection.Penetration, ref projection.InertiaA, ref projection.InertiaB, ref projection.Normal, ref accumulatedImpulses.Penetration0, ref accumulatedImpulses.Penetration1, ref accumulatedImpulses.Penetration2, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); }
public static void ApplyImpulse(ref PenetrationLimitOneBodyProjection projection, ref BodyInertias inertiaA, ref Vector3Wide normal, ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA) { var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass; Vector3Wide.Scale(ref normal, ref linearVelocityChangeA, out var correctiveVelocityALinearVelocity); Vector3Wide.Scale(ref projection.AngularA, ref correctiveImpulse, out var correctiveAngularImpulseA); Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref correctiveAngularImpulseA, ref inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity); Vector3Wide.Add(ref wsvA.LinearVelocity, ref correctiveVelocityALinearVelocity, out wsvA.LinearVelocity); Vector3Wide.Add(ref wsvA.AngularVelocity, ref correctiveVelocityAAngularVelocity, out wsvA.AngularVelocity); }
public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection, ref BodyVelocities wsvA, ref Vector <float> maximumImpulse, ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI) { Vector3Wide.Dot(wsvA.Angular, angularJacobianA, out var csvA); var negativeCSI = csvA * projection.EffectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse. var previousAccumulated = accumulatedImpulse; //The maximum force of friction depends upon the normal impulse. accumulatedImpulse = Vector.Min(maximumImpulse, Vector.Max(-maximumImpulse, accumulatedImpulse - negativeCSI)); correctiveCSI = accumulatedImpulse - previousAccumulated; }
public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection, ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Vector <float> maximumImpulse, ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI) { Vector3Wide.Dot(ref wsvA.AngularVelocity, ref angularJacobianA, out var csvA); Vector3Wide.Dot(ref wsvB.AngularVelocity, ref angularJacobianA, out var negatedCSVB); var negatedCSI = (csvA - negatedCSVB) * projection.EffectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse. var previousAccumulated = accumulatedImpulse; accumulatedImpulse = Vector.Min(maximumImpulse, Vector.Max(-maximumImpulse, accumulatedImpulse - negatedCSI)); correctiveCSI = accumulatedImpulse - previousAccumulated; }
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref PenetrationLimitOneBodyProjection projection, ref Vector3Wide normal, ref Vector <float> softnessImpulseScale, ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI) { //Note that we do NOT use pretransformed jacobians here; the linear jacobian sharing (normal) meant that we had the effective mass anyway. Vector3Wide.Dot(ref wsvA.LinearVelocity, ref normal, out var csvaLinear); Vector3Wide.Dot(ref wsvA.AngularVelocity, ref projection.AngularA, out var csvaAngular); //Compute negated version to avoid the need for an explicit negate. var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - projection.BiasVelocity) * projection.EffectiveMass; var previousAccumulated = accumulatedImpulse; accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse - negatedCSI); correctiveCSI = accumulatedImpulse - previousAccumulated; }
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularB, out var angularImpulseB); BodyVelocities correctiveVelocityA, correctiveVelocityB; Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular); Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out correctiveVelocityB.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out correctiveVelocityB.Angular); Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear); Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular); Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityB.Linear, out wsvB.Linear); //note subtract- we based it on the LinearA jacobian. Vector3Wide.Add(wsvB.Angular, correctiveVelocityB.Angular, out wsvB.Angular); }
public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide constraintSpaceImpulse) { Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi); Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change); Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular); Vector3Wide.Scale(constraintSpaceImpulse, inertiaA.InverseMass, out change); Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear); Vector3Wide.CrossWithoutOverlap(constraintSpaceImpulse, offsetB, out wsi); //note flip-negation Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaB.InverseInertiaTensor, out change); Vector3Wide.Add(velocityB.Angular, change, out velocityB.Angular); Vector3Wide.Scale(constraintSpaceImpulse, inertiaB.InverseMass, out change); Vector3Wide.Subtract(velocityB.Linear, change, out velocityB.Linear); //note subtraction; the jacobian is -I }
public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); //Note subtraction; jLinearB = -I. Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var csv); Vector3Wide.CrossWithoutOverlap(velocityA.Angular, offsetA, out var angularCSV); Vector3Wide.Add(csv, angularCSV, out csv); //Note reversed cross order; matches the jacobian -CrossMatrix(offsetB). Vector3Wide.CrossWithoutOverlap(offsetB, velocityB.Angular, out angularCSV); Vector3Wide.Add(csv, angularCSV, out csv); Vector3Wide.Subtract(biasVelocity, csv, out csv); Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out correctiveImpulse); Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softness); Vector3Wide.Subtract(correctiveImpulse, softness, out correctiveImpulse); }
public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { //Applying the impulse requires transforming the constraint space impulse into a world space velocity change. //The first step is to transform into a world space impulse, which requires transforming by the transposed jacobian //(transpose(jacobian) goes from world to constraint space, jacobian goes from constraint to world space). //That world space impulse is then converted to a corrective velocity change by scaling the impulse by the inverse mass/inertia. //As an optimization for constraints with smaller jacobians, the jacobian * (inertia or mass) transform is precomputed. BodyVelocities correctiveVelocityA, correctiveVelocityB; Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear); Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular); Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear); Vector3Wide.Scale(data.CSIToWSVAngularB, correctiveImpulse, out correctiveVelocityB.Angular); Vector3Wide.Add(correctiveVelocityA.Linear, wsvA.Linear, out wsvA.Linear); Vector3Wide.Add(correctiveVelocityA.Angular, wsvA.Angular, out wsvA.Angular); Vector3Wide.Add(correctiveVelocityB.Linear, wsvB.Linear, out wsvB.Linear); Vector3Wide.Add(correctiveVelocityB.Angular, wsvB.Angular, out wsvB.Angular); }
public static void ApplyImpulse(ref PenetrationLimitProjection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal, ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass; Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity); Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA); Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity); var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass; Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity); Vector3Wide.Scale(projection.AngularB, correctiveImpulse, out var correctiveAngularImpulseB); Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity); Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear); Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular); Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); //Note subtract; normal = -jacobianLinearB Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, out wsvB.Angular); }
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref Projection data, ref Jacobians jacobians, ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) { Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear); Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular); Vector2Wide.Add(csvaLinear, csvaAngular, out var csv); //Required corrective velocity is the negation of the current constraint space velocity. Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var negativeCSI); var previousAccumulated = accumulatedImpulse; Vector2Wide.Subtract(accumulatedImpulse, negativeCSI, out accumulatedImpulse); //The maximum force of friction depends upon the normal impulse. The maximum is supplied per iteration. Vector2Wide.Length(accumulatedImpulse, out var accumulatedMagnitude); //Note division by zero guard. var scale = Vector.Min(Vector <float> .One, maximumImpulse / Vector.Max(new Vector <float>(1e-16f), accumulatedMagnitude)); Vector2Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse); Vector2Wide.Subtract(accumulatedImpulse, previousAccumulated, out correctiveCSI); }
public void Solve(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z); var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. PenetrationLimit4.Solve(ref projection.Penetration, ref projection.InertiaA, ref projection.InertiaB, ref projection.Normal, ref accumulatedImpulses.Penetration0, ref accumulatedImpulses.Penetration1, ref accumulatedImpulses.Penetration2, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * projection.LeverArm0 + accumulatedImpulses.Penetration1 * projection.LeverArm1 + accumulatedImpulses.Penetration2 * projection.LeverArm2 + accumulatedImpulses.Penetration3 * projection.LeverArm3); TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); }
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Projection2Body1DOF projection, ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI) { //Take the world space velocity of each body into constraint space by transforming by the transpose(jacobian). //(The jacobian is a row vector by convention, while we treat our velocity vectors as a 12x1 row vector for the purposes of constraint space velocity calculation. //So we are multiplying v * JT.) //Then, transform it into an impulse by applying the effective mass. //Here, we combine the projection and impulse conversion into a precomputed value, i.e. v * (JT * softenedEffectiveMass). Vector3Wide.Dot(wsvA.Linear, projection.WSVtoCSILinearA, out var csiaLinear); Vector3Wide.Dot(wsvA.Angular, projection.WSVtoCSIAngularA, out var csiaAngular); Vector3Wide.Dot(wsvB.Linear, projection.WSVtoCSILinearB, out var csibLinear); Vector3Wide.Dot(wsvB.Angular, projection.WSVtoCSIAngularB, out var csibAngular); //Combine it all together, following: //constraint space impulse = (targetVelocity - currentVelocity) * softenedEffectiveMass //constraint space impulse = (bias - accumulatedImpulse * softness - wsv * JT) * softenedEffectiveMass //constraint space impulse = (bias * softenedEffectiveMass) - accumulatedImpulse * (softness * softenedEffectiveMass) - wsv * (JT * softenedEffectiveMass) var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); var previousAccumulated = accumulatedImpulse; accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse + csi); correctiveCSI = accumulatedImpulse - previousAccumulated; }
public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref TwistFrictionProjection projection, ref Vector <float> maximumImpulse, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA) { ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref angularJacobianA, ref inertiaA, ref correctiveCSI, ref wsvA); }
public static void Solve(ref Projection2Body1DOF projection, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref projection, ref correctiveCSI, ref wsvA, ref wsvB); }
public static void WarmStart(ref Projection2Body1DOF data, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. //To compensate for this, the accumulated impulse should be scaled if dt changes. ApplyImpulse(ref data, ref accumulatedImpulse, ref wsvA, ref wsvB); }
public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertias inertiaA, ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians); ComputeCorrectiveImpulse(ref wsvA, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref jacobians, ref inertiaA, ref correctiveCSI, ref wsvA); }
public void GatherCollidableBundle(int collidablesStartIndex, int count, out Vector <int> shapeIndices, out Vector <float> maximumExpansion, out RigidPoses poses, out BodyVelocities velocities) { Bodies.GatherDataForBounds(ref BodyIndices[collidablesStartIndex], count, out poses, out velocities, out shapeIndices, out maximumExpansion); }