public unsafe BoundingBoxWide(BoundingBox* boundingBoxes, Vector<int>[] masks) { Min = new Vector3Wide(ref boundingBoxes[0].Min); Max = new Vector3Wide(ref boundingBoxes[0].Max); for (int i = 1; i < Vector<float>.Count; ++i) { BoundingBoxWide wide = new BoundingBoxWide(ref boundingBoxes[i]); ConditionalSelect(ref masks[i], ref wide, ref this, out this); } }
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 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); Vector3Wide.Add(accumulatedImpulse, correctiveImpulse, out accumulatedImpulse); ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse); }
public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertias inertiaA, out Projection projection) { ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, out var jacobians); //Compute effective mass matrix contributions. Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA); Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA); //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) Symmetric2x2Wide.Add(linearContributionA, angularContributionA, out var inverseEffectiveMass); Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass); projection.OffsetA = offsetA; //Note that friction constraints have no bias velocity. They target zero velocity. }
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { //Work in the local space of the triangle, since it's quicker to transform the sphere position than the vertices of the triangle. Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB); Vector3Wide.Subtract(b.B, b.A, out var ab); Vector3Wide.Subtract(b.C, b.A, out var ac); //localOffsetA = -localOffsetB, so pa = triangle.A + localOffsetB. Vector3Wide.Add(b.A, localOffsetB, out var pa); Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal); Vector3Wide.Dot(localTriangleNormal, pa, out var paN); var collidingWithSolidSide = Vector.GreaterThan(paN, Vector <float> .Zero); if (Vector.EqualsAll(collidingWithSolidSide, Vector <int> .Zero)) { //No lanes can generate contacts due to the triangle's one sidedness. manifold.ContactExists = Vector <int> .Zero; return; } //EdgeAB plane test: (pa x ab) * (ab x ac) >= 0 //EdgeAC plane test: (ac x pa) * (ab x ac) >= 0 //Note that these are scaled versions of the barycentric coordinates. //To normalize them such that the weights of a point within the triangle equal 1, we just need to divide by dot(ab x ac, ab x ac). //In other words, to test the third edge plane, we can ensure that the unnormalized weights are both positive and sum to a value less than dot(ab x ac, ab x ac). //If a point is outside of an edge plane, we know that it's not in the face region or any other edge region. It could, however, be in an adjacent vertex region. //Vertex cases can be handled by clamping an edge case. //Further, note that for any query location, it is sufficient to only test one edge even if the point is outside two edge planes. If it's outside two edge planes, //that just means it's going to be on the shared vertex, so a clamped edge test captures the correct closest point. //So, at each edge, if the point is outside the plane, cache the edge. The last edge registering an outside result will be tested. Vector3Wide.CrossWithoutOverlap(pa, ab, out var paxab); Vector3Wide.CrossWithoutOverlap(ac, pa, out var acxpa); Vector3Wide.Dot(paxab, localTriangleNormal, out var edgePlaneTestAB); Vector3Wide.Dot(acxpa, localTriangleNormal, out var edgePlaneTestAC); Vector3Wide.Dot(localTriangleNormal, localTriangleNormal, out var triangleNormalLengthSquared); var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC; var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector <float> .Zero); var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector <float> .Zero); var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector <float> .Zero); var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC)); Vector3Wide localClosestOnTriangle; var negativeOne = new Vector <int>(-1); if (Vector.EqualsAny(Vector.BitwiseAnd(collidingWithSolidSide, outsideAnyEdge), negativeOne)) { //At least one lane detected a point outside of the triangle. Choose one edge which is outside as the representative. Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection); Vector3Wide.Subtract(b.C, b.B, out var bc); Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection); Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart); Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP); //This does some partially redundant work if the edge is AB or AC, but given that we didn't have bcbc or bcpb, it's fine. Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge); Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge); var edgeScale = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, -negativeOffsetDotEdge / edgeDotEdge)); Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge); Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge); Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle); } if (Vector.EqualsAny(Vector.AndNot(collidingWithSolidSide, outsideAnyEdge), negativeOne)) { //p + N * (pa * N) / ||N||^2 = N * (pa * N) / ||N||^2 - (-p) var nScale = paN / triangleNormalLengthSquared; Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane); Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace); Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle); } manifold.FeatureId = Vector.ConditionalSelect(outsideAnyEdge, Vector <int> .Zero, new Vector <int>(MeshReduction.FaceCollisionFlag)); //We'll be using the contact position to perform boundary smoothing; in order to find other triangles, the contact position has to be on the mesh surface. Matrix3x3Wide.TransformWithoutOverlap(localClosestOnTriangle, rB, out manifold.OffsetA); Vector3Wide.Add(manifold.OffsetA, offsetB, out manifold.OffsetA); Vector3Wide.Length(manifold.OffsetA, out var distance); //Note the normal is calibrated to point from B to A. var normalScale = new Vector <float>(-1) / distance; Vector3Wide.Scale(manifold.OffsetA, normalScale, out manifold.Normal); manifold.Depth = a.Radius - distance; //In the event that the sphere's center point is touching the triangle, the normal is undefined. In that case, the 'correct' normal would be the triangle's normal. //However, given that this is a pretty rare degenerate case and that we already treat triangle backfaces as noncolliding, we'll treat zero distance as a backface non-collision. manifold.ContactExists = Vector.BitwiseAnd( Vector.GreaterThan(distance, Vector <float> .Zero), Vector.BitwiseAnd( Vector.GreaterThanOrEqual(paN, Vector <float> .Zero), Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin))); }
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); }
public unsafe override void Initialize(ContentArchive content, Camera camera) { { SphereTriangleTester tester; SphereWide sphere = default; sphere.Broadcast(new Sphere(0.5f)); TriangleWide triangle = default; var a = new Vector3(0, 0, 0); var b = new Vector3(1, 0, 0); var c = new Vector3(0, 0, 1); //var center = (a + b + c) / 3f; //a -= center; //b -= center; //c -= center; triangle.Broadcast(new Triangle(a, b, c)); var margin = new Vector <float>(1f); Vector3Wide.Broadcast(new Vector3(1, -1, 0), out var offsetB); QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), out var orientationB); tester.Test(ref sphere, ref triangle, ref margin, ref offsetB, ref orientationB, Vector <float> .Count, out var manifold); } { CapsuleTriangleTester tester; CapsuleWide capsule = default; capsule.Broadcast(new Capsule(0.5f, 0.5f)); TriangleWide triangle = default; var a = new Vector3(0, 0, 0); var b = new Vector3(1, 0, 0); var c = new Vector3(0, 0, 1); //var center = (a + b + c) / 3f; //a -= center; //b -= center; //c -= center; triangle.Broadcast(new Triangle(a, b, c)); var margin = new Vector <float>(2f); Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB); QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA); QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); tester.Test(ref capsule, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector <float> .Count, out var manifold); } { BoxTriangleTester tester; BoxWide shape = default; shape.Broadcast(new Box(1f, 1f, 1f)); TriangleWide triangle = default; var a = new Vector3(0, 0, 0); var b = new Vector3(1, 0, 0); var c = new Vector3(0, 0, 1); //var center = (a + b + c) / 3f; //a -= center; //b -= center; //c -= center; triangle.Broadcast(new Triangle(a, b, c)); var margin = new Vector <float>(2f); Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB); QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA); QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); tester.Test(ref shape, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector <float> .Count, out var manifold); } { TrianglePairTester tester; TriangleWide a = default, b = default;
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 Vector <float> accumulatedImpulse2, ref Vector <float> accumulatedImpulse3, 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); ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration2, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse2, out var correctiveCSI2); ApplyImpulse(ref projection.Penetration2, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI2, ref wsvA, ref wsvB); ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration3, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse3, out var correctiveCSI3); ApplyImpulse(ref projection.Penetration3, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI3, ref wsvA, ref wsvB); }
public void GetPoint(int pointIndex, out Vector3 point) { BundleIndexing.GetBundleIndices(pointIndex, out var bundleIndex, out var innerIndex); Vector3Wide.ReadSlot(ref Points[bundleIndex], innerIndex, out point); }
public void GetPoint(HullVertexIndex pointIndex, out Vector3 point) { Vector3Wide.ReadSlot(ref Points[pointIndex.BundleIndex], pointIndex.InnerIndex, out point); }
public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var capsuleOrientation); Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation); Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(capsuleOrientation, hullOrientation, out var hullLocalCapsuleOrientation); ref var localCapsuleAxis = ref hullLocalCapsuleOrientation.Y;
public void Test( ref CapsuleWide a, ref CapsuleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { //Compute the closest points between the two line segments. No clamping to begin with. //We want to minimize distance = ||(a + da * ta) - (b + db * tb)||. //Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields: //ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db)) QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da); QuaternionWide.TransformUnitY(orientationB, out var db); Vector3Wide.Dot(da, offsetB, out var daOffsetB); Vector3Wide.Dot(db, offsetB, out var dbOffsetB); Vector3Wide.Dot(da, db, out var dadb); //Note potential division by zero when the axes are parallel. Arbitrarily clamp; near zero values will instead produce extreme values which get clamped to reasonable results. var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector <float>(1e-15f), Vector <float> .One - dadb * dadb); //tb = ta * (da * db) - db * (b - a) var tb = ta * dadb - dbOffsetB; //We cannot simply clamp the ta and tb values to the capsule line segments. Instead, project each line segment onto the other line segment, clamping against the target's interval. //That new clamped projected interval is the valid solution space on that line segment. We can clamp the t value by that interval to get the correctly bounded solution. //The projected intervals are: //B onto A: +-BHalfLength * (da * db) + da * offsetB //A onto B: +-AHalfLength * (da * db) - db * offsetB var absdadb = Vector.Abs(dadb); var bOntoAOffset = b.HalfLength * absdadb; var aOntoBOffset = a.HalfLength * absdadb; var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset)); var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset)); var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB)); var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB)); ta = Vector.Min(Vector.Max(ta, aMin), aMax); tb = Vector.Min(Vector.Max(tb, bMin), bMax); Vector3Wide.Scale(da, ta, out var closestPointOnA); Vector3Wide.Scale(db, tb, out var closestPointOnB); Vector3Wide.Add(closestPointOnB, offsetB, out closestPointOnB); //Note that normals are calibrated to point from B to A by convention. Vector3Wide.Subtract(closestPointOnA, closestPointOnB, out manifold.Normal); Vector3Wide.Length(manifold.Normal, out var distance); var inverseDistance = Vector <float> .One / distance; Vector3Wide.Scale(manifold.Normal, inverseDistance, out manifold.Normal); //In the event that the line segments are touching, the normal doesn't exist and we need an alternative. Any direction along the local horizontal (XZ) plane of either capsule //is valid. (Normals along the local Y axes are not guaranteed to be as quick of a path to separation due to nonzero line length.) var normalIsValid = Vector.GreaterThan(distance, new Vector <float>(1e-7f)); Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, xa, out manifold.Normal); //In the event that the two capsule axes are coplanar, we accept the whole interval as a source of contact. //As the axes drift away from coplanarity, the accepted interval rapidly narrows to zero length, centered on ta and tb. //We rate the degree of coplanarity based on the angle between the capsule axis and the plane defined by the box edge and contact normal: //sin(angle) = dot(da, (db x normal)/||db x normal||) //Finally, note that we are dealing with extremely small angles, and for small angles sin(angle) ~= angle, //and also that fade behavior is completely arbitrary, so we can directly use squared angle without any concern. //angle^2 ~= dot(da, (db x normal))^2 / ||db x normal||^2 //Note that if ||db x normal|| is the zero, then any da should be accepted as being coplanar because there is no restriction. ConditionalSelect away the discontinuity. Vector3Wide.CrossWithoutOverlap(db, manifold.Normal, out var planeNormal); Vector3Wide.LengthSquared(planeNormal, out var planeNormalLengthSquared); Vector3Wide.Dot(da, planeNormal, out var numeratorUnsquared); var squaredAngle = Vector.ConditionalSelect(Vector.LessThan(planeNormalLengthSquared, new Vector <float>(1e-10f)), Vector <float> .Zero, numeratorUnsquared * numeratorUnsquared / planeNormalLengthSquared); //Convert the squared angle to a lerp parameter. For squared angle from 0 to lowerThreshold, we should use the full interval (1). From lowerThreshold to upperThreshold, lerp to 0. const float lowerThresholdAngle = 0.01f; const float upperThresholdAngle = 0.05f; const float lowerThreshold = lowerThresholdAngle * lowerThresholdAngle; const float upperThreshold = upperThresholdAngle * upperThresholdAngle; var intervalWeight = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, (new Vector <float>(upperThreshold) - squaredAngle) * new Vector <float>(1f / (upperThreshold - lowerThreshold)))); //If the line segments intersect, even if they're coplanar, we would ideally stick to using a single point. Would be easy enough, //but we don't bother because it's such a weird and extremely temporary corner case. Not really worth handling. var weightedTa = ta - ta * intervalWeight; aMin = intervalWeight * aMin + weightedTa; aMax = intervalWeight * aMax + weightedTa; Vector3Wide.Scale(da, aMin, out manifold.OffsetA0); Vector3Wide.Scale(da, aMax, out manifold.OffsetA1); //In the coplanar case, there are two points. We need a method of computing depth which gives a reasonable result to the second contact. //Note that one of the two contacts should end up with a distance equal to the previously computed segment distance, so we're doing some redundant work here. //It's just easier to do that extra work than it would be to track which endpoint contributed the lower distance. //Unproject the final interval endpoints from a back onto b. //dot(offsetB + db * tb0, da) = ta0 //tb0 = (ta0 - daOffsetB) / dadb //distance0 = dot(a0 - (offsetB + tb0 * db), normal) //distance1 = dot(a1 - (offsetB + tb1 * db), normal) Vector3Wide.Dot(db, manifold.Normal, out var dbNormal); Vector3Wide.Subtract(manifold.OffsetA0, offsetB, out var offsetB0); Vector3Wide.Subtract(manifold.OffsetA1, offsetB, out var offsetB1); //Note potential division by zero. In that case, treat both projected points as the closest point. (Handled by the conditional select that chooses the previously computed distance.) var inverseDadb = Vector <float> .One / dadb; var projectedTb0 = Vector.Max(bMin, Vector.Min(bMax, (aMin - daOffsetB) * inverseDadb)); var projectedTb1 = Vector.Max(bMin, Vector.Min(bMax, (aMax - daOffsetB) * inverseDadb)); Vector3Wide.Dot(offsetB0, manifold.Normal, out var b0Normal); Vector3Wide.Dot(offsetB1, manifold.Normal, out var b1Normal); var capsulesArePerpendicular = Vector.LessThan(Vector.Abs(dadb), new Vector <float>(1e-7f)); var distance0 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b0Normal - dbNormal * projectedTb0); var distance1 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b1Normal - dbNormal * projectedTb1); var combinedRadius = a.Radius + b.Radius; manifold.Depth0 = combinedRadius - distance0; manifold.Depth1 = combinedRadius - distance1; //Apply the normal offset to the contact positions. var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius; var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius; Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA0, out var normalPush0); Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA1, out var normalPush1); Vector3Wide.Add(manifold.OffsetA0, normalPush0, out manifold.OffsetA0); Vector3Wide.Add(manifold.OffsetA1, normalPush1, out manifold.OffsetA1); manifold.FeatureId0 = Vector <int> .Zero; manifold.FeatureId1 = Vector <int> .One; var minimumAcceptedDepth = -speculativeMargin; manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth); manifold.Contact1Exists = Vector.BitwiseAnd( Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth), Vector.GreaterThan(aMax - aMin, new Vector <float>(1e-7f) * a.HalfLength)); //TODO: Since we added in the complexity of 2 contact support, this is probably large enough to benefit from working in the local space of one of the capsules. //Worth looking into later. }
public BoundingBoxWide(ref BoundingBox boundingBox) { Min = new Vector3Wide(ref boundingBox.Min); Max = new Vector3Wide(ref boundingBox.Max); }
public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwistFrictionProjection projection, ref Vector <float> maximumImpulse, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { 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 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 ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref Vector3Wide offsetB, out Jacobians jacobians) { //Two velocity constraints: //dot(velocity(p, A), tangentX) = dot(velocity(p, B), tangentX) //dot(velocity(p, A), tangentY) = dot(velocity(p, B), tangentY) //where velocity(p, A) is the velocity of a point p attached to object A. //velocity(p, A) = linearVelocityA + angularVelocityA x (p - positionA) = linearVelocityA + angularVelocityA x offsetA //so: //dot(velocity(p, A), tangentX) = dot(linearVelocityA, tangentX) + dot(angularVelocityA x offsetA, tangentX) //dot(velocity(p, A), tangentX) = dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA) //Restating the two constraints: //dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA) = dot(linearVelocityB, tangentX) + dot(offsetB x tangentX, angularVelocityB) //dot(linearVelocityA, tangentY) + dot(offsetA x tangentY, angularVelocityA) = dot(linearVelocityB, tangentY) + dot(offsetB x tangentY, angularVelocityB) //dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA) - dot(linearVelocityB, tangentX) - dot(offsetB x tangentX, angularVelocityB) = 0 //dot(linearVelocityA, tangentY) + dot(offsetA x tangentY, angularVelocityA) - dot(linearVelocityB, tangentY) - dot(offsetB x tangentY, angularVelocityB) = 0 //Since there are two constraints (2DOFs), there are two rows in the jacobian, which based on the above is: //jLinearA = [ tangentX ] // [ tangentY ] //jAngularA = [ offsetA x tangentX ] // [ offsetA x tangentY ] //jLinearB = [ -tangentX ] // [ -tangentY ] //jAngularB = [ -offsetB x tangentX ] = [ tangentX x offsetB ] // [ -offsetB x tangentY ] [ tangentY x offsetB ] //TODO: there would be a minor benefit in eliminating this copy manually, since it's very likely that the compiler won't. And it's probably also introducing more locals init. jacobians.LinearA.X = tangentX; jacobians.LinearA.Y = tangentY; Vector3Wide.CrossWithoutOverlap(ref offsetA, ref tangentX, out jacobians.AngularA.X); Vector3Wide.CrossWithoutOverlap(ref offsetA, ref tangentY, out jacobians.AngularA.Y); Vector3Wide.CrossWithoutOverlap(ref tangentX, ref offsetB, out jacobians.AngularB.X); Vector3Wide.CrossWithoutOverlap(ref tangentY, ref offsetB, out jacobians.AngularB.Y); }
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector <float> maximumRecoveryVelocity, ref Vector <float> positionError, float dt, float inverseDt, out Projection2Body1DOF projection) { //unsoftened effective mass = (J * M^-1 * JT)^-1 //where J is a constraintDOF x bodyCount*6 sized matrix, JT is its transpose, and for two bodies M^-1 is: //[inverseMassA, 0, 0, 0] //[0, inverseInertiaA, 0, 0] //[0, 0, inverseMassB, 0] //[0, 0, 0, inverseInertiaB] //The entries of J match up to this convention, containing the linear and angular components of each body in sequence, so for a 2 body 1DOF constraint J would look like: //[linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3] //Note that it is a row vector by convention. When transforming velocities from world space into constraint space, it is assumed that the velocity vector is organized as a //row vector matching up to the jacobian (that is, [linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]), so for a 2 body 2 DOF constraint, //worldVelocity * JT would be a [worldVelocity: 1x12] * [JT: 12x2], resulting in a 1x2 constraint space velocity row vector. //Similarly, when going from constraint space impulse to world space impulse in the above example, we would do [csi: 1x2] * [J: 2x12] to get a 1x12 world impulse row vector. //Note that the engine uses row vectors for all velocities and positions and so on. Rotation and inertia tensors are constructed for premultiplication. //In other words, unlike many of the presentations in the space, we use v * JT and csi * J instead of J * v and JT * csi. //There is no meaningful difference- the two conventions are just transpositions of each other. //(If you want to know how this stuff works, go read the constraint related presentations: http://box2d.org/downloads/ //Be mindful of the difference in conventions. You'll see J * v instead of v * JT, for example. Everything is still fundamentally the same, though.) //Due to the block structure of the mass matrix, we can handle each component separately and then sum the results. //For this 1DOF constraint, the result is a simple scalar. //Note that we store the intermediate results of J * M^-1 for use when projecting from constraint space impulses to world velocity changes. //If we didn't store those intermediate values, we could just scale the dot product of jacobians.LinearA with itself to save 4 multiplies. Vector3Wide.Scale(jacobians.LinearA, inertiaA.InverseMass, out projection.CSIToWSVLinearA); Vector3Wide.Scale(jacobians.LinearB, inertiaB.InverseMass, out projection.CSIToWSVLinearB); Vector3Wide.Dot(projection.CSIToWSVLinearA, jacobians.LinearA, out var linearA); Vector3Wide.Dot(projection.CSIToWSVLinearB, jacobians.LinearB, out var linearB); //The angular components are a little more involved; (J * I^-1) * JT is explicitly computed. Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularA, inertiaA.InverseInertiaTensor, out projection.CSIToWSVAngularA); Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularB, inertiaB.InverseInertiaTensor, out projection.CSIToWSVAngularB); Vector3Wide.Dot(projection.CSIToWSVAngularA, jacobians.AngularA, out var angularA); Vector3Wide.Dot(projection.CSIToWSVAngularB, jacobians.AngularB, out var angularB); //Now for a digression! //Softness is applied along the diagonal (which, for a 1DOF constraint, is just the only element). //Check the the ODE reference for a bit more information: http://ode.org/ode-latest-userguide.html#sec_3_8_0 //And also see Erin Catto's Soft Constraints presentation for more details: http://box2d.org/files/GDC2011/GDC2011_Catto_Erin_Soft_Constraints.pdf) //There are some very interesting tricks you can use here, though. //Our core tuning variables are the damping ratio and natural frequency. //Our runtime used variables are softness and an error reduction feedback scale.. //(For the following, I'll use the ODE terms CFM and ERP, constraint force mixing and error reduction parameter.) //So first, we need to get from damping ratio and natural frequency to stiffness and damping spring constants. //From there, we'll go to CFM/ERP. //Then, we'll create an expression for a softened effective mass matrix (i.e. one that takes into account the CFM term), //and an expression for the contraint force mixing term in the solve iteration. //Finally, compute ERP. //(And then some tricks.) //1) Convert from damping ratio and natural frequency to stiffness and damping constants. //The raw expressions are: //stiffness = effectiveMass * naturalFrequency^2 //damping = effectiveMass * 2 * dampingRatio * naturalFrequency //Rather than using any single object as the reference for the 'mass' term involved in this conversion, use the effective mass of the constraint. //In other words, we're dynamically picking the spring constants necessary to achieve the desired behavior for the current constraint configuration. //(See Erin Catto's presentation above for more details on this.) //(Note that this is different from BEPUphysics v1. There, users configured stiffness and damping constants. That worked okay, but people often got confused about //why constraints didn't behave the same when they changed masses. Usually it manifested as someone creating an incredibly high mass object relative to the default //stiffness/damping, and they'd post on the forum wondering why constraints were so soft. Basically, the defaults were another sneaky tuning factor to get wrong. //Since damping ratio and natural frequency define the behavior independent of the mass, this problem goes away- and it makes some other interesting things happen...) //2) Convert from stiffness and damping constants to CFM and ERP. //CFM = (stiffness * dt + damping)^-1 //ERP = (stiffness * dt) * (stiffness * dt + damping)^-1 //Or, to rephrase: //ERP = (stiffness * dt) * CFM //3) Use CFM and ERP to create a softened effective mass matrix and a force mixing term for the solve iterations. //Start with a base definition which we won't be deriving, the velocity constraint itself (stated as an equality constraint here): //This means 'world space velocity projected into constraint space should equal the velocity bias term combined with the constraint force mixing term'. //(The velocity bias term will be computed later- it's the position error scaled by the error reduction parameter, ERP. Position error is used to create a velocity motor goal.) //We're pulling back from the implementation of sequential impulses here, so rather than using the term 'accumulated impulse', we'll use 'lambda' //(which happens to be consistent with the ODE documentation covering the same topic). Lambda is impulse that satisfies the constraint. //wsv * JT = bias - lambda * CFM/dt //This can be phrased as: //currentVelocity = targetVelocity //Or: //goalVelocityChange = targetVelocity - currentVelocity //lambda = goalVelocityChange * effectiveMass //lambda = (targetVelocity - currentVelocity) * effectiveMass //lambda = (bias - lambda * CFM/dt - currentVelocity) * effectiveMass //Solving for lambda: //lambda = (bias - currentVelocity) * effectiveMass - lambda * CFM/dt * effectiveMass //lambda + lambda * CFM/dt * effectiveMass = (bias - currentVelocity) * effectiveMass //(lambda + lambda * CFM/dt * effectiveMass) * effectiveMass^-1 = bias - currentVelocity //lambda * effectiveMass^-1 + lambda * CFM/dt = bias - currentVelocity //lambda * (effectiveMass^-1 + CFM/dt) = bias - currentVelocity //lambda = (bias - currentVelocity) * (effectiveMass^-1 + CFM/dt)^-1 //lambda = (bias - wsv * JT) * (effectiveMass^-1 + CFM/dt)^-1 //In other words, we transform the velocity change (bias - wsv * JT) into the constraint-satisfying impulse, lambda, using a matrix (effectiveMass^-1 + CFM/dt)^-1. //That matrix is the softened effective mass: //softenedEffectiveMass = (effectiveMass^-1 + CFM/dt)^-1 //Here's where some trickiness occurs. (Be mindful of the distinction between the softened and unsoftened effective mass). //Start by substituting CFM into the softened effective mass definition: //CFM/dt = (stiffness * dt + damping)^-1 / dt = (dt * (stiffness * dt + damping))^-1 = (stiffness * dt^2 + damping*dt)^-1 //softenedEffectiveMass = (effectiveMass^-1 + (stiffness * dt^2 + damping * dt)^-1)^-1 //Now substitute the definitions of stiffness and damping, treating the scalar components as uniform scaling matrices of dimension equal to effectiveMass: //softenedEffectiveMass = (effectiveMass^-1 + ((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency) * dt)^-1)^-1 //Combine the inner effectiveMass coefficients, given matrix multiplication distributes over addition: //softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2) + effectiveMass * (2 * dampingRatio * naturalFrequency * dt))^-1)^-1 //softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1)^-1 //Apply the inner matrix inverse: //softenedEffectiveMass = (effectiveMass^-1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1)^-1 //Once again, combine coefficients of the inner effectiveMass^-1 terms: //softenedEffectiveMass = ((1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1) * effectiveMass^-1)^-1 //Apply the inverse again: //softenedEffectiveMass = effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 //So, to put it another way- because CFM is based on the effective mass, applying it to the effective mass results in a simple downscale. //What has been gained? Consider what happens in the solve iteration. //We take the velocity error: //velocityError = bias - accumulatedImpulse * CFM/dt - wsv * JT //and convert it to a corrective impulse with the effective mass: //impulse = (bias - accumulatedImpulse * CFM/dt - wsv * JT) * softenedEffectiveMass //The effective mass distributes over the set: //impulse = bias * softenedEffectiveMass - accumulatedImpulse * CFM/dt * softenedEffectiveMass - wsv * JT * softenedEffectiveMass //Focus on the CFM term: //-accumulatedImpulse * CFM/dt * softenedEffectiveMass //What is CFM/dt * softenedEffectiveMass? Substitute. //(stiffness * dt^2 + damping * dt)^-1 * softenedEffectiveMass //((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass //Combine terms: //(effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass //Apply inverse: //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * softenedEffectiveMass //Expand softened effective mass from earlier: //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 //Cancel effective masses: (!) //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 //Because CFM was created from effectiveMass, the CFM/dt * effectiveMass term is actually independent of the effectiveMass! //The remaining expression is still a matrix, but fortunately it is a simple uniform scaling matrix that we can store and apply as a single scalar. //4) How do you compute ERP? //ERP = (stiffness * dt) * CFM //ERP = (stiffness * dt) * (stiffness * dt + damping)^-1 //ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * naturalFrequency^2) * dt + (effectiveMass * 2 * dampingRatio * naturalFrequency))^-1 //Combine denominator terms: //ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency))^-1 //Apply denominator inverse: //ERP = ((effectiveMass * naturalFrequency^2) * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 * effectiveMass^-1 //Uniform scaling matrices commute: //ERP = (naturalFrequency^2 * dt) * effectiveMass * effectiveMass^-1 * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 //Cancellation! //ERP = (naturalFrequency^2 * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 //ERP = (naturalFrequency * dt) * (naturalFrequency * dt + 2 * dampingRatio)^-1 //ERP is a simple scalar, independent of mass. //5) So we can compute CFM, ERP, the softened effective mass matrix, and we have an interesting shortcut on the constraint force mixing term of the solve iterations. //Is there anything more that can be done? You bet! //Let's look at the post-distribution impulse computation again: //impulse = bias * effectiveMass - accumulatedImpulse * CFM/dt * effectiveMass - wsv * JT * effectiveMass //During the solve iterations, the only quantities that vary are the accumulated impulse and world space velocities. So the rest can be precomputed. //bias * effectiveMass, //CFM/dt * effectiveMass, //JT * effectiveMass //In other words, we bypass the intermediate velocity state and go directly from source velocities to an impulse. //Note the sizes of the precomputed types above: //bias * effective mass is the same size as bias (vector with dimension equal to constrained DOFs) //CFM/dt * effectiveMass is a single scalar regardless of constrained DOFs, //JT * effectiveMass is the same size as JT //But note that we no longer need to load the effective mass! It is implicit. //The resulting computation is: //impulse = a - accumulatedImpulse * b - wsv * c //two DOF-width adds (add/subtract), one DOF-width multiply, and a 1xDOF * DOFx12 jacobian-sized transform. //Compare to; //(bias - accumulatedImpulse * CFM/dt - wsv * JT) * effectiveMass //two DOF-width adds (add/subtract), one DOF width multiply, a 1xDOF * DOFx12 jacobian-sized transform, and a 1xDOF * DOFxDOF transform. //In other words, we shave off a whole 1xDOF * DOFxDOF transform per iteration. //So, taken in isolation, this is a strict win both in terms of memory and the amount of computation. //Unfortunately, it's not quite so simple- jacobians are ALSO used to transform the impulse into world space so that it can be used to change the body velocities. //We still need to have those around. So while we no longer store the effective mass, our jacobian has sort of been duplicated. //But wait, there's more! //That process looks like: //wsv += impulse * J * M^-1 //So while we need to store something here, we can take advantage of the fact that we aren't using the jacobian anywhere else (it's replaced by the JT * effectiveMass term above). //Precompute J*M^-1, too. //So you're still loading a jacobian-sized matrix, but you don't need to load M^-1! That saves you 20 scalars. (3x3 + 1 + 3x3 + 1, inverse inertia and mass of two bodies.) //That saves you the multiplication of (impulse * J) * M^-1, which is 6 multiplies and 6 dot products. //Note that this optimization's value depends on the number of constrained DOFs. //Net memory change, opt vs no opt, in scalars: //1DOF: costs 1x12, saves 1x1 effective mass and the 20 scalar M^-1: -9 //2DOF: costs 2x12, saves 2x2 effective mass and the 20 scalar M^-1: 0 //3DOF: costs 3x12, saves 3x3 effective mass and the 20 scalar M^-1: 7 //4DOF: costs 4x12, saves 4x4 effective mass and the 20 scalar M^-1: 12 //5DOF: costs 5x12, saves 5x5 effective mass and the 20 scalar M^-1: 15 //6DOF: costs 6x12, saves 6x6 effective mass and the 20 scalar M^-1: 16 //Net compute savings, opt vs no opt: //DOF savings = 1xDOF * DOFxDOF (DOF DOFdot products), 2 1x3 * scalar (6 multiplies), 2 1x3 * 3x3 (6 3dot products) // = (DOF*DOF multiplies + DOF*(DOF-1) adds) + (6 multiplies) + (18 multiplies + 12 adds) // = DOF*DOF + 24 multiplies, DOF*DOF-DOF + 12 adds //1DOF: 25 multiplies, 12 adds //2DOF: 28 multiplies, 14 adds //3DOF: 33 multiplies, 18 adds //4DOF: 40 multiplies, 24 adds //5DOF: 49 multiplies, 32 adds //6DOF: 60 multiplies, 42 adds //So does our 'optimization' actually do anything useful? //In 1 or 2 DOF constraints, it's a win with no downsides. //3+ are difficult to determine. //This depends on heavily on the machine's SIMD width. You do every lane's ALU ops in parallel, but the loads are still fundamentally bound by memory bandwidth. //The loads are coherent, at least- no gathers on this stuff. But I wouldn't be surprised if 3DOF+ constraints end up being faster *without* the pretransformations on wide SIMD. //This is just something that will require testing. //(Also, note that large DOF jacobians are often very sparse. Consider the jacobians used by a 6DOF weld joint. You could likely do special case optimizations to reduce the //load further. It is unlikely that you could find a way to do the same to JT * effectiveMass. J * M^-1 might have some savings, though. But J*M^-1 isn't *sparser* //than J by itself, so the space savings are limited. As long as you precompute, the above load requirement offset will persist.) //Good news, though! There are a lot of 1DOF and 2DOF constraints where this is an unambiguous win. //We'll start with the unsoftened effective mass, constructed from the contributions computed above: var effectiveMass = Vector <float> .One / (linearA + linearB + angularA + angularB); SpringSettingsWide.ComputeSpringiness(ref springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); var softenedEffectiveMass = effectiveMass * effectiveMassCFMScale; //Note that we use a bit of a hack when computing the bias velocity- even if our damping ratio/natural frequency implies a strongly springy response //that could cause a significant velocity overshoot, we apply an arbitrary clamping value to keep it reasonable. //This is useful for a variety of inequality constraints (like contacts) because you don't always want them behaving as true springs. var biasVelocity = Vector.Min(positionError * positionErrorToVelocity, maximumRecoveryVelocity); projection.BiasImpulse = biasVelocity * softenedEffectiveMass; //Precompute the wsv * (JT * softenedEffectiveMass) term. //Note that we store it in a Vector3Wide as if it's a row vector, but this is really a column (because JT is a column vector). //So we're really storing (JT * softenedEffectiveMass)T = softenedEffectiveMassT * J. //Since this constraint is 1DOF, the softenedEffectiveMass is a scalar and the order doesn't matter. //In the solve iterations, the WSVtoCSI term will be transposed during transformation, //resulting in the proper wsv * (softenedEffectiveMassT * J)T = wsv * (JT * softenedEffectiveMass). //You'll see this pattern repeated in higher DOF constraints. We explicitly compute softenedEffectiveMassT * J, and then apply the transpose in the solves. //(Why? Because creating a Matrix3x2 and Matrix2x3 and 4x3 and 3x4 and 5x3 and 3x5 and so on just doubles the number of representations with little value.) Vector3Wide.Scale(jacobians.LinearA, softenedEffectiveMass, out projection.WSVtoCSILinearA); Vector3Wide.Scale(jacobians.AngularA, softenedEffectiveMass, out projection.WSVtoCSIAngularA); Vector3Wide.Scale(jacobians.LinearB, softenedEffectiveMass, out projection.WSVtoCSILinearB); Vector3Wide.Scale(jacobians.AngularB, softenedEffectiveMass, out projection.WSVtoCSIAngularB); }
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal, ref Contact4PrestepData prestep, float dt, float inverseDt, out Projection projection) { //We directly take the prestep data here since the jacobians and error don't undergo any processing. //The contact penetration constraint takes the form: //dot(positionA + offsetA, N) >= dot(positionB + offsetB, N) //Or: //dot(positionA + offsetA, N) - dot(positionB + offsetB, N) >= 0 //dot(positionA + offsetA - positionB - offsetB, N) >= 0 //where positionA and positionB are the center of mass positions of the bodies offsetA and offsetB are world space offsets from the center of mass to the contact, //and N is a unit length vector calibrated to point from B to A. (The normal pointing direction is important; it changes the sign.) //In practice, we'll use the collision detection system's penetration depth instead of trying to recompute the error here. //So, treating the normal as constant, the velocity constraint is: //dot(d/dt(positionA + offsetA - positionB - offsetB), N) >= 0 //dot(linearVelocityA + d/dt(offsetA) - linearVelocityB - d/dt(offsetB)), N) >= 0 //The velocity of the offsets are defined by the angular velocity. //dot(linearVelocityA + angularVelocityA x offsetA - linearVelocityB - angularVelocityB x offsetB), N) >= 0 //dot(linearVelocityA, N) + dot(angularVelocityA x offsetA, N) - dot(linearVelocityB, N) - dot(angularVelocityB x offsetB), N) >= 0 //Use the properties of the scalar triple product: //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) - dot(linearVelocityB, N) - dot(offsetB x N, angularVelocityB) >= 0 //Bake in the negations: //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(-offsetB x N, angularVelocityB) >= 0 //A x B = -B x A: //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(N x offsetB, angularVelocityB) >= 0 //And there you go, the jacobians! //linearA: N //angularA: offsetA x N //linearB: -N //angularB: N x offsetB //Note that we leave the penetration depth as is, even when it's negative. Speculative contacts! Vector3Wide.CrossWithoutOverlap(prestep.OffsetA0, normal, out projection.Penetration0.AngularA); Vector3Wide.Subtract(prestep.OffsetA0, prestep.OffsetB, out var offsetB0); Vector3Wide.CrossWithoutOverlap(normal, offsetB0, out projection.Penetration0.AngularB); Vector3Wide.CrossWithoutOverlap(prestep.OffsetA1, normal, out projection.Penetration1.AngularA); Vector3Wide.Subtract(prestep.OffsetA1, prestep.OffsetB, out var offsetB1); Vector3Wide.CrossWithoutOverlap(normal, offsetB1, out projection.Penetration1.AngularB); Vector3Wide.CrossWithoutOverlap(prestep.OffsetA2, normal, out projection.Penetration2.AngularA); Vector3Wide.Subtract(prestep.OffsetA2, prestep.OffsetB, out var offsetB2); Vector3Wide.CrossWithoutOverlap(normal, offsetB2, out projection.Penetration2.AngularB); Vector3Wide.CrossWithoutOverlap(prestep.OffsetA3, normal, out projection.Penetration3.AngularA); Vector3Wide.Subtract(prestep.OffsetA3, prestep.OffsetB, out var offsetB3); Vector3Wide.CrossWithoutOverlap(normal, offsetB3, out projection.Penetration3.AngularB); //effective mass Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularA, inertiaA.InverseInertiaTensor, out var angularA0); Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularB, inertiaB.InverseInertiaTensor, out var angularB0); Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularA, inertiaA.InverseInertiaTensor, out var angularA1); Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularB, inertiaB.InverseInertiaTensor, out var angularB1); Symmetric3x3Wide.VectorSandwich(projection.Penetration2.AngularA, inertiaA.InverseInertiaTensor, out var angularA2); Symmetric3x3Wide.VectorSandwich(projection.Penetration2.AngularB, inertiaB.InverseInertiaTensor, out var angularB2); Symmetric3x3Wide.VectorSandwich(projection.Penetration3.AngularA, inertiaA.InverseInertiaTensor, out var angularA3); Symmetric3x3Wide.VectorSandwich(projection.Penetration3.AngularB, inertiaB.InverseInertiaTensor, out var angularB3); //Linear effective mass contribution notes: //1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar. //2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass. SpringSettingsWide.ComputeSpringiness(ref prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); var linear = inertiaA.InverseMass + inertiaB.InverseMass; //Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway. projection.Penetration0.EffectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0); projection.Penetration1.EffectiveMass = effectiveMassCFMScale / (linear + angularA1 + angularB1); projection.Penetration2.EffectiveMass = effectiveMassCFMScale / (linear + angularA2 + angularB2); projection.Penetration3.EffectiveMass = effectiveMassCFMScale / (linear + angularA3 + angularB3); //If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1. var inverseDtVector = new Vector <float>(inverseDt); projection.Penetration0.BiasVelocity = Vector.Min(prestep.PenetrationDepth0 * inverseDtVector, Vector.Min(prestep.PenetrationDepth0 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity)); projection.Penetration1.BiasVelocity = Vector.Min(prestep.PenetrationDepth1 * inverseDtVector, Vector.Min(prestep.PenetrationDepth1 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity)); projection.Penetration2.BiasVelocity = Vector.Min(prestep.PenetrationDepth2 * inverseDtVector, Vector.Min(prestep.PenetrationDepth2 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity)); projection.Penetration3.BiasVelocity = Vector.Min(prestep.PenetrationDepth3 * inverseDtVector, Vector.Min(prestep.PenetrationDepth3 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity)); }
public void ComputeSupport(ref CapsuleWide shape, ref Matrix3x3Wide orientation, ref Vector3Wide direction, out Vector3Wide support) { Vector3Wide.Scale(orientation.Y, shape.HalfLength, out support); Vector3Wide.Negate(support, out var negated); Vector3Wide.Dot(orientation.Y, direction, out var dot); var shouldNegate = Vector.LessThan(dot, Vector <float> .Zero); Vector3Wide.ConditionalSelect(shouldNegate, negated, support, out support); }
public void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal) { //Compute the closest points between the two line segments. No clamping to begin with. //We want to minimize distance = ||(a + da * ta) - (b + db * tb)||. //Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields: //ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db)) QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da); QuaternionWide.TransformUnitY(orientationB, out var db); Vector3Wide.Dot(da, offsetB, out var daOffsetB); Vector3Wide.Dot(db, offsetB, out var dbOffsetB); Vector3Wide.Dot(da, db, out var dadb); //Note potential division by zero when the axes are parallel. Arbitrarily clamp; near zero values will instead produce extreme values which get clamped to reasonable results. var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector <float>(1e-15f), Vector <float> .One - dadb * dadb); //tb = ta * (da * db) - db * (b - a) var tb = ta * dadb - dbOffsetB; //We cannot simply clamp the ta and tb values to the capsule line segments. Instead, project each line segment onto the other line segment, clamping against the target's interval. //That new clamped projected interval is the valid solution space on that line segment. We can clamp the t value by that interval to get the correctly bounded solution. //The projected intervals are: //B onto A: +-BHalfLength * (da * db) + da * offsetB //A onto B: +-AHalfLength * (da * db) - db * offsetB var absdadb = Vector.Abs(dadb); var bOntoAOffset = b.HalfLength * absdadb; var aOntoBOffset = a.HalfLength * absdadb; var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset)); var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset)); var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB)); var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB)); ta = Vector.Min(Vector.Max(ta, aMin), aMax); tb = Vector.Min(Vector.Max(tb, bMin), bMax); Vector3Wide.Scale(da, ta, out closestA); Vector3Wide.Scale(db, tb, out var closestB); Vector3Wide.Add(closestB, offsetB, out closestB); Vector3Wide.Subtract(closestA, closestB, out normal); Vector3Wide.Length(normal, out distance); var inverseDistance = Vector <float> .One / distance; Vector3Wide.Scale(normal, inverseDistance, out normal); Vector3Wide.Scale(normal, a.Radius, out var aOffset); Vector3Wide.Subtract(closestA, aOffset, out closestA); distance = distance - a.Radius - b.Radius; intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero); }
public static unsafe void Test() { const int iterations = 10000000; { var vectors = stackalloc Vector3[Vector<float>.Count]; //var vectors = new Vector3[Vector<float>.Count]; for (int j = 0; j < Vector<float>.Count; ++j) { vectors[j] = new Vector3(); } float[] x, y, z; x = new float[Vector<float>.Count]; y = new float[Vector<float>.Count]; z = new float[Vector<float>.Count]; Vector3Wide acc = new Vector3Wide(x, y, z); Vector3Wide v1 = new Vector3Wide(x, y, z); Vector3Wide.Add(ref v1, ref acc, out acc); var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < iterations; ++i) { for (int j = 0; j < Vector<float>.Count; ++j) { x[j] = vectors[j].X; y[j] = vectors[j].Y; z[j] = vectors[j].Z; } v1 = new Vector3Wide(x, y, z); Vector3Wide.Add(ref v1, ref acc, out acc); } var endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; Console.WriteLine($"* Time: {endTime - startTime}, acc: {acc}, size: {Vector<float>.Count}"); } { Vector3 v = new Vector3(0, 1, 2); Vector3Wide acc = new Vector3Wide(ref v); Vector3Wide v1 = new Vector3Wide(ref v); Vector3Wide.Add(ref v1, ref acc, out acc); var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < iterations; ++i) { v1 = new Vector3Wide(ref v); Vector3Wide.Add(ref v1, ref acc, out acc); } var endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; Console.WriteLine($"* Single Time: {endTime - startTime}, acc: {acc}, size: {Vector<float>.Count}"); } { Vector3 a, b, c, d; a = b = c = d = new Vector3(); Vector3Width4 acc = new Vector3Width4(); Vector3Width4 v1 = new Vector3Width4(ref a, ref b, ref c, ref d); Vector3Width4.Add(ref v1, ref acc, out acc); var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < iterations; ++i) { v1 = new Vector3Width4(ref a, ref b, ref c, ref d); Vector3Width4.Add(ref v1, ref acc, out acc); } var endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; Console.WriteLine($"Fixed Time: {endTime - startTime}, acc: {acc}"); } }
static void Select(ref Vector <float> distanceSquared, ref Vector3Wide localNormal, ref Vector <float> distanceSquaredCandidate, ref Vector3Wide localNormalCandidate) { var useCandidate = Vector.LessThan(distanceSquaredCandidate, distanceSquared); distanceSquared = Vector.Min(distanceSquaredCandidate, distanceSquared); Vector3Wide.ConditionalSelect(useCandidate, localNormalCandidate, localNormal, out localNormal); }
public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); Matrix3x3Wide.CreateIdentity(out var identity); Vector3Wide.Length(localOffsetA, out var centerDistance); Vector3Wide.Scale(localOffsetA, Vector <float> .One / centerDistance, out var initialNormal); var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-8f)); initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X); initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y); initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); var sphereSupportFinder = default(SphereSupportFinder); ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(a.Radius, hullEpsilonScale); DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, PhysicsSphere, SphereWide, SphereSupportFinder> .FindMinimumDepth( b, a, localOffsetA, identity, ref hullSupportFinder, ref sphereSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, -speculativeMargin, out var depth, out var localNormal, out var closestOnHull); Matrix3x3Wide.TransformWithoutOverlap(closestOnHull, hullOrientation, out var hullToContact); Matrix3x3Wide.TransformWithoutOverlap(localNormal, hullOrientation, out manifold.Normal); Vector3Wide.Add(hullToContact, offsetB, out manifold.OffsetA); manifold.FeatureId = Vector <int> .Zero; manifold.Depth = depth; manifold.ContactExists = Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin); }
public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, out Jacobians jacobians) { //TODO: there would be a minor benefit in eliminating this copy manually, since it's very likely that the compiler won't. And it's probably also introducing more locals init. jacobians.LinearA.X = tangentX; jacobians.LinearA.Y = tangentY; Vector3Wide.CrossWithoutOverlap(offsetA, tangentX, out jacobians.AngularA.X); Vector3Wide.CrossWithoutOverlap(offsetA, tangentY, out jacobians.AngularA.Y); }
public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); }
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 unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var boxOrientation); Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation); Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(boxOrientation, hullOrientation, out var hullLocalBoxOrientation); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); Vector3Wide.Length(localOffsetA, out var centerDistance); Vector3Wide.Scale(localOffsetA, Vector <float> .One / centerDistance, out var initialNormal); var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-8f)); initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X); initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y); initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); var boxSupportFinder = default(BoxSupportFinder); ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(Vector.Max(a.HalfWidth, Vector.Max(a.HalfHeight, a.HalfLength)), hullEpsilonScale); var depthThreshold = -speculativeMargin; DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Box, BoxWide, BoxSupportFinder> .FindMinimumDepth( b, a, localOffsetA, hullLocalBoxOrientation, ref hullSupportFinder, ref boxSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, depthThreshold, out var depth, out var localNormal, out var closestOnHull); inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(depth, depthThreshold)); //Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front. manifold.Contact0Exists = default; manifold.Contact1Exists = default; manifold.Contact2Exists = default; manifold.Contact3Exists = default; if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero)) { //No contacts generated. return; } //Identify the box face. Matrix3x3Wide.TransformByTransposedWithoutOverlap(localNormal, hullLocalBoxOrientation, out var localNormalInA); Vector3Wide.Abs(localNormalInA, out var absLocalNormalInA); var useX = Vector.BitwiseAnd(Vector.GreaterThan(absLocalNormalInA.X, absLocalNormalInA.Y), Vector.GreaterThan(absLocalNormalInA.X, absLocalNormalInA.Z)); var useY = Vector.AndNot(Vector.GreaterThan(absLocalNormalInA.Y, absLocalNormalInA.Z), useX); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.X, hullLocalBoxOrientation.Z, out var boxFaceNormal); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.Y, boxFaceNormal, out boxFaceNormal); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.Y, hullLocalBoxOrientation.X, out var boxFaceX); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.Z, boxFaceX, out boxFaceX); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.Z, hullLocalBoxOrientation.Y, out var boxFaceY); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.X, boxFaceY, out boxFaceY); var negateFace = Vector.ConditionalSelect(useX, Vector.GreaterThan(localNormalInA.X, Vector <float> .Zero), Vector.ConditionalSelect(useY, Vector.GreaterThan(localNormalInA.Y, Vector <float> .Zero), Vector.GreaterThan(localNormalInA.Z, Vector <float> .Zero))); Vector3Wide.ConditionallyNegate(negateFace, ref boxFaceNormal); //Winding is important; flip the face bases if necessary. Vector3Wide.ConditionallyNegate(Vector.OnesComplement(negateFace), ref boxFaceX); var boxFaceHalfWidth = Vector.ConditionalSelect(useX, a.HalfHeight, Vector.ConditionalSelect(useY, a.HalfLength, a.HalfWidth)); var boxFaceHalfHeight = Vector.ConditionalSelect(useX, a.HalfLength, Vector.ConditionalSelect(useY, a.HalfWidth, a.HalfHeight)); var boxFaceNormalOffset = Vector.ConditionalSelect(useX, a.HalfWidth, Vector.ConditionalSelect(useY, a.HalfHeight, a.HalfLength)); Vector3Wide.Scale(boxFaceNormal, boxFaceNormalOffset, out var boxFaceCenterOffset); Vector3Wide.Add(boxFaceCenterOffset, localOffsetA, out var boxFaceCenter); Vector3Wide.Scale(boxFaceX, boxFaceHalfWidth, out var boxFaceXOffset); Vector3Wide.Scale(boxFaceY, boxFaceHalfHeight, out var boxFaceYOffset); Vector3Wide.Subtract(boxFaceCenter, boxFaceXOffset, out var v0); Vector3Wide.Add(boxFaceCenter, boxFaceXOffset, out var v1); Vector3Wide.Subtract(v0, boxFaceYOffset, out var v00); Vector3Wide.Add(v0, boxFaceYOffset, out var v01); Vector3Wide.Subtract(v1, boxFaceYOffset, out var v10); Vector3Wide.Add(v1, boxFaceYOffset, out var v11); //To find the contact manifold, we'll clip the box edges against the hull face as usual, but we're dealing with potentially //distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull. Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices); var boundingPlaneEpsilon = 1e-3f * epsilonScale; //There can be no more than 8 contacts (provided there are no numerical errors); 2 per box edge. var candidates = stackalloc ManifoldCandidateScalar[8]; for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex) { if (inactiveLanes[slotIndex] < 0) { continue; } ref var hull = ref b.Hulls[slotIndex]; ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out var slotFaceNormal, out var slotLocalNormal, out var bestFaceIndex); //Test each face edge plane against the box face. //Note that we do not use the faceNormal x edgeOffset edge plane, but rather edgeOffset x localNormal. //The faces are wound counterclockwise in right handed coordinates. //X is 00->10; Y is 10->11; Z is 11->01; W is 01->00. ref var v00Slot = ref GatherScatter.GetOffsetInstance(ref v00, slotIndex);
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 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 WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA) { ApplyImpulse(ref angularJacobianA, 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 Vector <float> accumulatedImpulse2, ref Vector <float> accumulatedImpulse3, 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); ApplyImpulse(ref projection.Penetration2, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse2, ref wsvA, ref wsvB); ApplyImpulse(ref projection.Penetration3, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse3, ref wsvA, ref wsvB); }
public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, ref BodyVelocities velocities, float dt, ref Vector <float> maximumRadius, ref Vector <float> maximumAngularExpansion, ref Vector <float> maximumExpansion) { /* * If an object sitting on a plane had a raw (unexpanded) AABB that is just barely above the plane, no contacts would be generated. * If the velocity of the object would shove it down into the plane in the next frame, then it would generate contacts in the next frame- and, * potentially, this cycle would repeat and cause jitter. * * To solve this, there are a couple of options: * 1) Introduce an 'allowed penetration' so that objects can overlap a little bit. This tends to confuse people a little bit when they notice it, * and in some circumstances objects can be seen settling into the allowed penetration slowly. It looks a bit odd. * 2) Make contact constraints fight to maintain zero penetration depth, but expand the bounding box with velocity and allow contacts to be generated speculatively- * contacts with negative penetration depth. * #2 is a form of continuous collision detection, but it's handy for general contact stability too. * In this version of the engine, all objects generate speculative contacts by default, though only within a per-collidable-tuned 'speculative margin'. * It's kind of like BEPUphysics v1's AllowedPenetration, except inverted. Speculative contacts that fall within the speculative margin- * that is, those with negative depth of a magnitude less than the margin- are kept. * * So, a user could choose to have a very large speculative margin, and the speculative contact generation would provide a form of continuous collision detection. * The main purpose, though, is just contact stability. With this in isolation, there's no strong reason to expand the bounding box more than the speculative margin. * This is the 'discrete' mode. * * However, consider what would happen if an object A with high velocity and this 'discrete' mode was headed towards an object B in a 'continuous' mode. * Object B only expands its bounding box by its own velocity, and object A doesn't expand beyond its speculative margin. The collision between A and B could easily be missed. * To account for this, there is an intermediate mode- 'passive'- where the bounding box is allowed to expand beyond the margin, * but no further continuous collision detection is performed. * * The fully continuous modes fully expand the bounding boxes. Notably, the inner sphere continuous collision detection mode could get by with less, * but it would be pretty confusing to have the same kind of missed collision possibility if the other object in the pair was a substepping object. * Two different inner sphere modes could be offered, but I'm unsure about the usefulness versus the complexity. * * (Note that there ARE situations where a bounding box which contains the full unconstrained motion will fail to capture constrained motion. * Consider object A flying at high speed to impact the stationary object B, which sits next to another stationary object C. * Object B's bounding box doesn't overlap with object C's bounding box- they're both stationary, so there's no velocity expansion. But during one frame, * object A slams into B, and object B's velocity during that frame now forces it to tunnel all the way through C unimpeded, because no contacts were generated between B and C. * There are ways to address this- all of which are a bit expensive- but CCD as implemented is not a hard guarantee. * It's a 'best effort' that compromises with performance. Later on, if it's really necessary, we could consider harder guarantees with higher costs, but... * given that no one seemed to have much of an issue with v1's rather limited CCD, it'll probably be fine.) * * So, how is the velocity expansion calculated? * There's two parts, linear and angular. * * Linear is pretty simple- expand the bounding box in the direction of linear displacement (linearVelocity * dt). */ Vector <float> vectorDt = new Vector <float>(dt); Vector3Wide.Scale(ref velocities.LinearVelocity, ref vectorDt, out var linearDisplacement); var zero = Vector <float> .Zero; Vector3Wide.Min(ref zero, ref linearDisplacement, out var minDisplacement); Vector3Wide.Max(ref zero, ref linearDisplacement, out var maxDisplacement); /* * Angular requires a bit more care. Since the goal is to create a tight bound, simply using a v = w * r approximation isn't ideal. A slightly tighter can be found: * 1) The maximum displacement along ANY axis during an intermediate time is equal to the distance from a starting position at MaximumRadius * to the position of that point at the intermediate time. * 2) The expansion cannot exceed the maximum radius, so angular deltas greater than pi/3 do not need to be considered. * (An expansion equal to the maximum radius would result in an equilateral triangle, which has an angle of 60 degrees in each corner.) * Larger values can simply be clamped. * 3) The largest displacement along any axis, at any time, is the distance from the starting position to the position at dt. Note that this only holds because of the clamp: * if the angle was allowed to wrap around, it the distance would start to go down again. * 4) position(time) = {radius * sin(angular speed * time), radius * cos(angular speed * time)} * 5) largest expansion required = ||position(dt) - position(0)|| = sqrt(2 * radius^2 * (1 - cos(dt * w))) * 6) Don't have any true SIMD sin function, but we can approximate it using a taylor series, like: cos(x) = 1 - x^2 / 2! + x^4 / 4! - x^6 / 6! * 7) Note that the cosine approximation should stop at a degree where it is smaller than the true value of cosine for the interval 0 to pi/3: this guarantees that the distance, * which is larger when the cosine is smaller, is conservative and fully bounds the angular motion. * * Why do this extra work? * 1) The bounding box calculation phase, as a part of the pose integration phase, tends to be severely memory bound. * Spending a little of ALU time to get a smaller bounding box isn't a big concern, even though it includes a couple of sqrts. * An extra few dozen ALU cycles is unlikely to meaningfully change the execution time. * 2) Shrinking the bounding box reduces the number of collision pairs. Collision pairs are expensive- many times more expensive than the cost of shrinking the bounding box. */ Vector3Wide.Length(ref velocities.AngularVelocity, out var angularVelocityMagnitude); var a = Vector.Min(angularVelocityMagnitude * vectorDt, new Vector <float>(MathHelper.Pi / 3f)); var a2 = a * a; var a4 = a2 * a2; var a6 = a4 * a2; var cosAngleMinusOne = a2 * new Vector <float>(-1f / 2f) + a4 * new Vector <float>(1f / 24f) - a6 * new Vector <float>(1f / 720f); //Note that it's impossible for angular motion to cause an increase in bounding box size beyond (maximumRadius-minimumRadius) on any given axis. //That value, or a conservative approximation, is stored as the maximum angular expansion. var angularExpansion = Vector.Min(maximumAngularExpansion, Vector.SquareRoot(new Vector <float>(-2f) * maximumRadius * maximumRadius * cosAngleMinusOne)); Vector3Wide.Subtract(ref minDisplacement, ref angularExpansion, out minDisplacement); Vector3Wide.Add(ref maxDisplacement, ref angularExpansion, out maxDisplacement); //The maximum expansion passed into this function is the speculative margin for discrete mode collidables, and ~infinity for passive or continuous ones. var negativeMaximum = -maximumExpansion; Vector3Wide.Max(ref negativeMaximum, ref minDisplacement, out minDisplacement); Vector3Wide.Min(ref maximumExpansion, ref maxDisplacement, out maxDisplacement); Vector3Wide.Add(ref min, ref minDisplacement, out min); Vector3Wide.Add(ref max, ref maxDisplacement, out max); //Note that this is an area that will need to adapt to changes to the body pose representation. If you use a 32 bit fixed point position or a 64 bit double position, //the bounding box calculation needs to be aware. (In the more extreme cases, so does the broad phase. The amount of conservative padding needed for a 32 bit bounding box //with 64 bit positions is silly.) }
public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, ref projection.OffsetB, out var jacobians); ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref jacobians, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref wsvB); }