public void SolveIteration() { Vector3Width4 linearVelocityA = new Vector3Width4(ref VelocitiesA0.LinearVelocity, ref VelocitiesA1.LinearVelocity, ref VelocitiesA2.LinearVelocity, ref VelocitiesA3.LinearVelocity); Vector3Width4 angularVelocityA = new Vector3Width4(ref VelocitiesA0.AngularVelocity, ref VelocitiesA1.AngularVelocity, ref VelocitiesA2.AngularVelocity, ref VelocitiesA3.AngularVelocity); Vector3Width4 linearVelocityB = new Vector3Width4(ref VelocitiesB0.LinearVelocity, ref VelocitiesB1.LinearVelocity, ref VelocitiesB2.LinearVelocity, ref VelocitiesB3.LinearVelocity); Vector3Width4 angularVelocityB = new Vector3Width4(ref VelocitiesB0.AngularVelocity, ref VelocitiesB1.AngularVelocity, ref VelocitiesB2.AngularVelocity, ref VelocitiesB3.AngularVelocity); for (int i = 0; i < ConstraintCount; ++i) { Constraints[i].SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); } //for (int i = 0; i < 4; ++i) //{ // a.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); //} //a.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); //b.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); //c.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); //d.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); Vector3Width4.Transpose(ref linearVelocityA, out VelocitiesA0.LinearVelocity, out VelocitiesA1.LinearVelocity, out VelocitiesA2.LinearVelocity, out VelocitiesA3.LinearVelocity); Vector3Width4.Transpose(ref linearVelocityB, out VelocitiesB0.LinearVelocity, out VelocitiesB1.LinearVelocity, out VelocitiesB2.LinearVelocity, out VelocitiesB3.LinearVelocity); Vector3Width4.Transpose(ref angularVelocityA, out VelocitiesA0.AngularVelocity, out VelocitiesA1.AngularVelocity, out VelocitiesA2.AngularVelocity, out VelocitiesA3.AngularVelocity); Vector3Width4.Transpose(ref angularVelocityB, out VelocitiesB0.AngularVelocity, out VelocitiesB1.AngularVelocity, out VelocitiesB2.AngularVelocity, out VelocitiesB3.AngularVelocity); }
internal static unsafe void Transpose(ref Vector3Width4 source, Vector3 *result) { result[0] = new Vector3(source.X.X, source.Y.X, source.Z.X); result[1] = new Vector3(source.X.Y, source.Y.Y, source.Z.Y); result[2] = new Vector3(source.X.Z, source.Y.Z, source.Z.Z); result[3] = new Vector3(source.X.W, source.Y.W, source.Z.W); }
void ApplyImpulse(float lambda, ref Vector3Width4 velocities) { Vector3Width4 change; Vector3Width4.Multiply(ref JacobiansIT, lambda, out change); Vector3Width4.Subtract(ref velocities, ref change, out velocities); }
public void SolveIteration() { Vector3Width4 linearVelocityA = new Vector3Width4(ref VelocitiesA0.LinearVelocity, ref VelocitiesA1.LinearVelocity, ref VelocitiesA2.LinearVelocity, ref VelocitiesA3.LinearVelocity); Vector3Width4 angularVelocityA = new Vector3Width4(ref VelocitiesA0.AngularVelocity, ref VelocitiesA1.AngularVelocity, ref VelocitiesA2.AngularVelocity, ref VelocitiesA3.AngularVelocity); Vector3Width4 linearVelocityB = new Vector3Width4(ref VelocitiesB0.LinearVelocity, ref VelocitiesB1.LinearVelocity, ref VelocitiesB2.LinearVelocity, ref VelocitiesB3.LinearVelocity); Vector3Width4 angularVelocityB = new Vector3Width4(ref VelocitiesB0.AngularVelocity, ref VelocitiesB1.AngularVelocity, ref VelocitiesB2.AngularVelocity, ref VelocitiesB3.AngularVelocity); Vector4 linearA, angularA, linearB, angularB; Vector3Width4.Dot(ref LinearJacobianA, ref linearVelocityA, out linearA); Vector3Width4.Dot(ref AngularJacobianA, ref angularVelocityA, out angularA); Vector3Width4.Dot(ref LinearJacobianB, ref linearVelocityB, out linearB); Vector3Width4.Dot(ref AngularJacobianB, ref angularVelocityB, out angularB); var lambda = EffectiveMass * (linearA + angularA + linearB + angularB + PenetrationBias - AccumulatedImpulse * Softness); var previous = AccumulatedImpulse; AccumulatedImpulse = Vector4.Max(Vector4.Zero, AccumulatedImpulse + lambda); lambda = AccumulatedImpulse - previous; ApplyImpulse(ref lambda, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); Vector3Width4.Transpose(ref linearVelocityA, out VelocitiesA0.LinearVelocity, out VelocitiesA1.LinearVelocity, out VelocitiesA2.LinearVelocity, out VelocitiesA3.LinearVelocity); Vector3Width4.Transpose(ref linearVelocityB, out VelocitiesB0.LinearVelocity, out VelocitiesB1.LinearVelocity, out VelocitiesB2.LinearVelocity, out VelocitiesB3.LinearVelocity); Vector3Width4.Transpose(ref angularVelocityA, out VelocitiesA0.AngularVelocity, out VelocitiesA1.AngularVelocity, out VelocitiesA2.AngularVelocity, out VelocitiesA3.AngularVelocity); Vector3Width4.Transpose(ref angularVelocityB, out VelocitiesB0.AngularVelocity, out VelocitiesB1.AngularVelocity, out VelocitiesB2.AngularVelocity, out VelocitiesB3.AngularVelocity); }
internal static unsafe void Transpose(ref Vector3Width4 source, out Vector3 a, out Vector3 b, out Vector3 c, out Vector3 d) { a = new Vector3(source.X.X, source.Y.X, source.Z.X); b = new Vector3(source.X.Y, source.Y.Y, source.Z.Y); c = new Vector3(source.X.Z, source.Y.Z, source.Z.Z); d = new Vector3(source.X.W, source.Y.W, source.Z.W); }
public void Prestep(float inverseDt) { //Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia. //Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together. //Otherwise you're multiplying cache misses for no reason! var inverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass); var inverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass); var inverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse); var inverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse); Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position); Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position); for (int i = 0; i < ConstraintCount; ++i) { Constraints[i].Prestep(inverseDt, ref inverseMassA, ref inverseMassB, ref inverseInertiaTensorA, ref inverseInertiaTensorB, ref positionA, ref positionB); } //for (int i = 0; i < 4; ++i) //{ // a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB); //} //a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB); //b.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB); //c.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB); //d.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB); }
public void Prestep(float inverseDt) { //C = dot(Pa - Pb, N) > 0 //Jacobians: //LinearA: N //AngularA: cross(OffsetPa, N) //LinearB: -N //AngularB: -cross(OffsetPb, N) //var positionA = new Vector3Width4(); //var positionB = new Vector3Width4(); //Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia. //Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together. //Otherwise you're multiplying cache misses for no reason! var InverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass); var InverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass); var InverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse); var InverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse); Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position); Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position); LinearJacobianA = ContactNormal; Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB); Vector3Width4 offsetA, offsetB; Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA); Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB); Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA); Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse //Allow velocity that closes a gap, and apply penetration correction against positive depth. //Bounciness not yet included. PenetrationBias = ContactPenetration * inverseDt; PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f)); //The inertia tensor is in world space, so no jacobian transformation is required. Vector3Width4.Multiply(ref LinearJacobianA, ref InverseMassA, out LinearJacobianITA); Vector3Width4.Multiply(ref LinearJacobianB, ref InverseMassB, out LinearJacobianITB); Matrix3x3Width4.Transform(ref AngularJacobianA, ref InverseInertiaTensorA, out AngularJacobianITA); Matrix3x3Width4.Transform(ref AngularJacobianB, ref InverseInertiaTensorB, out AngularJacobianITB); Vector4 angularContributionA, angularContributionB; Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA); Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB); var inverseEffectiveMass = InverseMassA + InverseMassB + angularContributionA + angularContributionB; Vector4 CollisionSoftness = new Vector4(5); Softness = CollisionSoftness * inverseEffectiveMass * inverseDt; EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass); }
public void Prestep(float inverseDt, ref Vector4 inverseMassA, ref Vector4 inverseMassB, ref Matrix3x3Width4 inverseInertiaTensorA, ref Matrix3x3Width4 inverseInertiaTensorB, ref Vector3Width4 positionA, ref Vector3Width4 positionB) { //C = dot(Pa - Pb, N) > 0 //Jacobians: //LinearA: N //AngularA: cross(OffsetPa, N) //LinearB: -N //AngularB: -cross(OffsetPb, N) //var positionA = new Vector3Width4(); //var positionB = new Vector3Width4(); LinearJacobianA = ContactNormal; Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB); Vector3Width4 offsetA, offsetB; Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA); Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB); Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA); Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse //Allow velocity that closes a gap, and apply penetration correction against positive depth. //Bounciness not yet included. PenetrationBias = ContactPenetration * inverseDt; PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f)); //The inertia tensor is in world space, so no jacobian transformation is required. Vector3Width4.Multiply(ref LinearJacobianA, ref inverseMassA, out LinearJacobianITA); Vector3Width4.Multiply(ref LinearJacobianB, ref inverseMassB, out LinearJacobianITB); Matrix3x3Width4.Transform(ref AngularJacobianA, ref inverseInertiaTensorA, out AngularJacobianITA); Matrix3x3Width4.Transform(ref AngularJacobianB, ref inverseInertiaTensorB, out AngularJacobianITB); Vector4 angularContributionA, angularContributionB; Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA); Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB); var inverseEffectiveMass = inverseMassA + inverseMassB + angularContributionA + angularContributionB; Vector4 CollisionSoftness = new Vector4(5); Softness = CollisionSoftness * inverseEffectiveMass * inverseDt; EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass); }
void ApplyImpulse(ref Vector4 lambda) { Vector3Width4 linearChangeA, linearChangeB; Vector3Width4.Multiply(ref LinearJacobianITA, ref lambda, out linearChangeA); Vector3Width4.Multiply(ref LinearJacobianITB, ref lambda, out linearChangeB); Vector3 linearChangeA0, linearChangeA1, linearChangeA2, linearChangeA3; Vector3 linearChangeB0, linearChangeB1, linearChangeB2, linearChangeB3; Vector3Width4.Transpose(ref linearChangeA, out linearChangeA0, out linearChangeA1, out linearChangeA2, out linearChangeA3); Vector3Width4.Transpose(ref linearChangeB, out linearChangeB0, out linearChangeB1, out linearChangeB2, out linearChangeB3); //World inertia available, so no need for extra transforms. Vector3Width4 angularChangeA, angularChangeB; Vector3Width4.Multiply(ref AngularJacobianITA, ref lambda, out angularChangeA); Vector3Width4.Multiply(ref AngularJacobianITB, ref lambda, out angularChangeB); Vector3 angularChangeA0, angularChangeA1, angularChangeA2, angularChangeA3; Vector3 angularChangeB0, angularChangeB1, angularChangeB2, angularChangeB3; Vector3Width4.Transpose(ref angularChangeA, out angularChangeA0, out angularChangeA1, out angularChangeA2, out angularChangeA3); Vector3Width4.Transpose(ref angularChangeB, out angularChangeB0, out angularChangeB1, out angularChangeB2, out angularChangeB3); VelocitiesA0.LinearVelocity -= linearChangeA0; VelocitiesA0.AngularVelocity -= angularChangeA0; VelocitiesA1.LinearVelocity -= linearChangeA1; VelocitiesA1.AngularVelocity -= angularChangeA1; VelocitiesA2.LinearVelocity -= linearChangeA2; VelocitiesA2.AngularVelocity -= angularChangeA2; VelocitiesA3.LinearVelocity -= linearChangeA3; VelocitiesA3.AngularVelocity -= angularChangeA3; VelocitiesB0.LinearVelocity -= linearChangeB0; VelocitiesB0.AngularVelocity -= angularChangeB0; VelocitiesB1.LinearVelocity -= linearChangeB1; VelocitiesB1.AngularVelocity -= angularChangeB1; VelocitiesB2.LinearVelocity -= linearChangeB2; VelocitiesB2.AngularVelocity -= angularChangeB2; VelocitiesB3.LinearVelocity -= linearChangeB3; VelocitiesB3.AngularVelocity -= angularChangeB3; }
public void SolveIteration() { Vector3Width4 velocities = new Vector3Width4(ref ConnectionA.LinearVelocity, ref ConnectionA.AngularVelocity, ref ConnectionB.LinearVelocity, ref ConnectionB.AngularVelocity); Vector4 velocityContributions; Vector3Width4.Dot(ref velocities, ref Jacobians, out velocityContributions); var lambda = EffectiveMass * ( velocityContributions.X + velocityContributions.Y + velocityContributions.Z + velocityContributions.W + PenetrationBias - AccumulatedImpulse * Softness); var previous = AccumulatedImpulse; AccumulatedImpulse = Math.Max(0, AccumulatedImpulse + lambda); lambda = AccumulatedImpulse - previous; ApplyImpulse(lambda, ref velocities); Vector3Width4.Transpose(ref velocities, out ConnectionA.LinearVelocity, out ConnectionA.AngularVelocity, out ConnectionB.LinearVelocity, out ConnectionB.AngularVelocity); }
void ApplyImpulse(ref Vector4 lambda, ref Vector3Width4 linearVelocityA, ref Vector3Width4 angularVelocityA, ref Vector3Width4 linearVelocityB, ref Vector3Width4 angularVelocityB) { Vector3Width4 linearChangeA, linearChangeB; Vector3Width4.Multiply(ref LinearJacobianITA, ref lambda, out linearChangeA); Vector3Width4.Multiply(ref LinearJacobianITB, ref lambda, out linearChangeB); Vector3Width4.Add(ref linearVelocityA, ref linearChangeA, out linearVelocityA); Vector3Width4.Add(ref linearVelocityB, ref linearChangeB, out linearVelocityB); //World inertia available, so no need for extra transforms. Vector3Width4 angularChangeA, angularChangeB; Vector3Width4.Multiply(ref AngularJacobianITA, ref lambda, out angularChangeA); Vector3Width4.Multiply(ref AngularJacobianITB, ref lambda, out angularChangeB); Vector3Width4.Add(ref angularVelocityA, ref angularChangeA, out angularVelocityA); Vector3Width4.Add(ref angularVelocityB, ref angularChangeB, out angularVelocityB); }
public unsafe static void Test() { var identityMatrix = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 0, 1) }; var a = new RigidBody { Position = new Vector3(0, 0, 0), Orientation = identityMatrix, InertiaTensorInverse = identityMatrix, InverseMass = 1, LinearVelocity = new Vector3(0, 0, 0) }; var b = new RigidBody { Position = new Vector3(0, 1, 0), Orientation = identityMatrix, InertiaTensorInverse = identityMatrix, InverseMass = 1, LinearVelocity = new Vector3(0, 0, 0) }; RigidBody *aBodies = stackalloc RigidBody[4]; RigidBody *bBodies = stackalloc RigidBody[4]; for (int i = 0; i < 4; ++i) { aBodies[i] = a; bBodies[i] = b; } Vector3 up = Vector3.UnitY; var contactPosition = new Vector3Width4(); var contactNormal = new Vector3Width4(ref up, ref up, ref up, ref up); var contactPenetration = new Vector4(); var constraint = new VectorizedManifoldConstraint { BodyA0 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() }, BodyA1 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() }, BodyA2 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() }, BodyA3 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() }, BodyB0 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) }, BodyB1 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) }, BodyB2 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) }, BodyB3 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) }, VelocitiesB0 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) }, VelocitiesB1 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) }, VelocitiesB2 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) }, VelocitiesB3 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) }, //a = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }, //b = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }, //c = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }, //d = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration } }; constraint.ConstraintCount = 4; var pointer = stackalloc VectorizedManifoldPenetrationConstraint[constraint.ConstraintCount]; constraint.Constraints = pointer; //constraint.Constraints = new VectorizedManifoldPenetrationConstraint[constraintCount]; for (int i = 0; i < constraint.ConstraintCount; ++i) { constraint.Constraints[i] = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }; } float dt = 1 / 60f; float inverseDt = 1 / dt; constraint.Prestep(inverseDt); constraint.WarmStart(); constraint.SolveIteration(); var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < VectorizedConstraintTest.TestCount / constraint.ConstraintCount; ++i) { constraint.Prestep(inverseDt); constraint.WarmStart(); for (int iterationIndex = 0; iterationIndex < VectorizedConstraintTest.IterationCount; ++iterationIndex) { constraint.SolveIteration(); } } var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; Console.WriteLine($"Vectorized Manifold: {endtime - startTime}"); }
public static void Multiply(ref Vector3Width4 a, float b, out Vector3Width4 result) { result.X = a.X * b; result.Y = a.Y * b; result.Z = a.Z * b; }
public static void Cross(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result) { result.X = a.Y * b.Z - a.Z * b.Y; result.Y = a.Z * b.X - a.X * b.Z; result.Z = a.X * b.Y - a.Y * b.X; }
public void WarmStart(ref Vector3Width4 linearVelocityA, ref Vector3Width4 angularVelocityA, ref Vector3Width4 linearVelocityB, ref Vector3Width4 angularVelocityB) { ApplyImpulse(ref AccumulatedImpulse, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); }
public static void Add(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result) { result.X = a.X + b.X; result.Y = a.Y + b.Y; result.Z = a.Z + b.Z; }
internal static unsafe void Transpose(ref Vector3Width4 source, Vector3* result) { result[0] = new Vector3(source.X.X, source.Y.X, source.Z.X); result[1] = new Vector3(source.X.Y, source.Y.Y, source.Z.Y); result[2] = new Vector3(source.X.Z, source.Y.Z, source.Z.Z); result[3] = new Vector3(source.X.W, source.Y.W, source.Z.W); }
public static void Subtract(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result) { result.X = a.X - b.X; result.Y = a.Y - b.Y; result.Z = a.Z - b.Z; }
public void SolveIteration(ref Vector3Width4 linearVelocityA, ref Vector3Width4 angularVelocityA, ref Vector3Width4 linearVelocityB, ref Vector3Width4 angularVelocityB) { Vector4 linearA, angularA, linearB, angularB; Vector3Width4.Dot(ref LinearJacobianA, ref linearVelocityA, out linearA); Vector3Width4.Dot(ref AngularJacobianA, ref angularVelocityA, out angularA); Vector3Width4.Dot(ref LinearJacobianB, ref linearVelocityB, out linearB); Vector3Width4.Dot(ref AngularJacobianB, ref angularVelocityB, out angularB); var lambda = EffectiveMass * (linearA + angularA + linearB + angularB + PenetrationBias - AccumulatedImpulse * Softness); var previous = AccumulatedImpulse; AccumulatedImpulse = Vector4.Max(Vector4.Zero, AccumulatedImpulse + lambda); lambda = AccumulatedImpulse - previous; ApplyImpulse(ref lambda, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); }
public static void Transform(ref Vector3Width4 v, ref Matrix3x3Width4 m, out Vector3Width4 result) { result.X = v.X * m.M11 + v.Y * m.M21 + v.Z * m.M31; result.Y = v.X * m.M12 + v.Y * m.M22 + v.Z * m.M32; result.Z = v.X * m.M13 + v.Y * m.M23 + v.Z * m.M33; }
internal static void Dot(ref Vector3Width4 a, ref Vector3Width4 b, out Vector4 result) { result = a.X * b.X + a.Y * b.Y + a.Z * b.Z; }
public static void Negate(ref Vector3Width4 v, out Vector3Width4 result) { result.X = -v.X; result.Y = -v.Y; result.Z = -v.Z; }
public static unsafe void Test() { var identityMatrix = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 0, 1) }; var a = new RigidBody { Position = new Vector3(0, 0, 0), Orientation = identityMatrix, InertiaTensorInverse = identityMatrix, InverseMass = 1, LinearVelocity = new Vector3(0, 0, 0) }; var b = new RigidBody { Position = new Vector3(0, 1, 0), Orientation = identityMatrix, InertiaTensorInverse = identityMatrix, InverseMass = 1, LinearVelocity = new Vector3(0, 0, 0) }; RigidBody* aBodies = stackalloc RigidBody[4]; RigidBody* bBodies = stackalloc RigidBody[4]; for (int i = 0; i < 4; ++i) { aBodies[i] = a; bBodies[i] = b; } Vector3 up = Vector3.UnitY; var contactPosition = new Vector3Width4(); var contactNormal = new Vector3Width4(ref up, ref up, ref up, ref up); var contactPenetration = new Vector4(); var constraint = new VectorizedManifoldConstraint { BodyA0 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() }, BodyA1 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() }, BodyA2 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() }, BodyA3 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() }, BodyB0 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) }, BodyB1 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) }, BodyB2 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) }, BodyB3 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) }, VelocitiesB0 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) }, VelocitiesB1 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) }, VelocitiesB2 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) }, VelocitiesB3 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) }, //a = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }, //b = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }, //c = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }, //d = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration } }; constraint.ConstraintCount = 4; var pointer = stackalloc VectorizedManifoldPenetrationConstraint[constraint.ConstraintCount]; constraint.Constraints = pointer; //constraint.Constraints = new VectorizedManifoldPenetrationConstraint[constraintCount]; for (int i = 0; i < constraint.ConstraintCount; ++i) { constraint.Constraints[i] = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }; } float dt = 1 / 60f; float inverseDt = 1 / dt; constraint.Prestep(inverseDt); constraint.WarmStart(); constraint.SolveIteration(); var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < VectorizedConstraintTest.TestCount / constraint.ConstraintCount; ++i) { constraint.Prestep(inverseDt); constraint.WarmStart(); for (int iterationIndex = 0; iterationIndex < VectorizedConstraintTest.IterationCount; ++iterationIndex) { constraint.SolveIteration(); } } var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; Console.WriteLine($"Vectorized Manifold: {endtime - startTime}"); }