public void Prestep(float inverseDt) { //Single constraint version. //C = dot(Pa - Pb, N) > 0 //Jacobians: //LinearA: N //AngularA: cross(OffsetPa, N) //LinearB: -N //AngularB: -cross(OffsetPb, N) LinearJacobianA = ContactNormal; LinearJacobianB = -ContactNormal; var offsetA = ContactPosition - ConnectionA.Position; var offsetB = ContactPosition - ConnectionB.Position; //Note scalar implementation; JIT doesn't do shuffles yet. Vector3Ex.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA); Vector3Ex.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 = -Math.Min(Math.Min(PenetrationBias, PenetrationBias * 0.2f), 0.2f); LinearJacobianITA = LinearJacobianA * ConnectionA.InverseMass; LinearJacobianITB = LinearJacobianB * ConnectionB.InverseMass; Matrix3x3.Transform(ref AngularJacobianA, ref ConnectionA.InertiaTensorInverse, out AngularJacobianITA); Matrix3x3.Transform(ref AngularJacobianB, ref ConnectionB.InertiaTensorInverse, out AngularJacobianITB); float inverseEffectiveMass = ConnectionA.InverseMass + ConnectionB.InverseMass + Vector3.Dot(AngularJacobianITA, AngularJacobianITA) + Vector3.Dot(AngularJacobianITB, AngularJacobianITB); const float CollisionSoftness = 5; Softness = CollisionSoftness * inverseEffectiveMass * inverseDt; EffectiveMass = 1f / (Softness + inverseEffectiveMass); }
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! linearA0 = ContactNormal; linearA1 = ContactNormal; linearA2 = ContactNormal; linearA3 = ContactNormal; linearB0 = -ContactNormal; linearB1 = -ContactNormal; linearB2 = -ContactNormal; linearB3 = -ContactNormal; Vector3 offsetA0, offsetB0, offsetA1, offsetB1, offsetA2, offsetB2, offsetA3, offsetB3; offsetA0 = ContactPosition - a0.Position; offsetA1 = ContactPosition - a1.Position; offsetA2 = ContactPosition - a2.Position; offsetA3 = ContactPosition - a3.Position; offsetB0 = ContactPosition - b0.Position; offsetB1 = ContactPosition - b1.Position; offsetB2 = ContactPosition - b2.Position; offsetB3 = ContactPosition - b3.Position; //Oof. All of this is scalar. Vector3Ex.Cross(ref offsetA0, ref ContactNormal, out angularA0); Vector3Ex.Cross(ref offsetA1, ref ContactNormal, out angularA1); Vector3Ex.Cross(ref offsetA2, ref ContactNormal, out angularA2); Vector3Ex.Cross(ref offsetA3, ref ContactNormal, out angularA3); Vector3Ex.Cross(ref ContactNormal, ref offsetB0, out angularB0);// note negation->parameter reverse Vector3Ex.Cross(ref ContactNormal, ref offsetB1, out angularB1); Vector3Ex.Cross(ref ContactNormal, ref offsetB2, out angularB2); Vector3Ex.Cross(ref ContactNormal, ref offsetB3, out angularB3); var contactPenetrations = new Vector4(ContactPenetration); //Allow velocity that closes a gap, and apply penetration correction against positive depth. //Bounciness not yet included. PenetrationBias = contactPenetrations * inverseDt; PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f)); linearITA0 = linearA0 * a0.InverseMass; linearITA1 = linearA1 * a1.InverseMass; linearITA2 = linearA2 * a2.InverseMass; linearITA3 = linearA3 * a3.InverseMass; linearITB0 = linearB0 * b0.InverseMass; linearITB1 = linearB1 * b1.InverseMass; linearITB2 = linearB2 * b2.InverseMass; linearITB3 = linearB3 * b3.InverseMass; //You might look at this and wonder, 'why is this sitting in the middle?' //Because it's 15-30% faster than other obvious spots. // :) :) :) :) //Optimizing Compiler. var inverseMassA = new Vector4(a0.InverseMass, a1.InverseMass, a2.InverseMass, a3.InverseMass); var inverseMassB = new Vector4(b0.InverseMass, b1.InverseMass, b2.InverseMass, b3.InverseMass); //The inertia tensor is in world space, so no jacobian transformation is required. Matrix3x3.Transform(ref angularA0, ref a0.InertiaTensorInverse, out angularITA0); Matrix3x3.Transform(ref angularB0, ref b0.InertiaTensorInverse, out angularITB0); Matrix3x3.Transform(ref angularA1, ref a1.InertiaTensorInverse, out angularITA1); Matrix3x3.Transform(ref angularB1, ref b1.InertiaTensorInverse, out angularITB1); Matrix3x3.Transform(ref angularA2, ref a2.InertiaTensorInverse, out angularITA2); Matrix3x3.Transform(ref angularB2, ref b2.InertiaTensorInverse, out angularITB2); Matrix3x3.Transform(ref angularA3, ref a3.InertiaTensorInverse, out angularITA3); Matrix3x3.Transform(ref angularB3, ref b3.InertiaTensorInverse, out angularITB3); Vector4 angularContributionsA = new Vector4(Vector3.Dot(angularITA0, angularITA0), Vector3.Dot(angularITA1, angularITA1), Vector3.Dot(angularITA2, angularITA2), Vector3.Dot(angularITA3, angularITA3)); Vector4 angularContributionsB = new Vector4(Vector3.Dot(angularITB0, angularITB0), Vector3.Dot(angularITB1, angularITB1), Vector3.Dot(angularITB2, angularITB2), Vector3.Dot(angularITB3, angularITB3)); var inverseEffectiveMass = inverseMassA + inverseMassB + angularContributionsA + angularContributionsB; Vector4 CollisionSoftness = new Vector4(5); Softness = CollisionSoftness * inverseEffectiveMass * inverseDt; EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass); }
public static void Test() { Vector3 v1 = new Vector3(0, 0, 1); Vector3 v2 = new Vector3(0, 0, 2); Vector3 v3 = new Vector3(0, 0, 3); Vector3 v4 = new Vector3(0, 0, 4); Vector3 v5 = new Vector3(0, 0, 5); Vector3 v6 = new Vector3(0, 0, 6); Vector3 v7 = new Vector3(0, 0, 7); Vector3 v8 = new Vector3(0, 0, 8); Matrix3x3 m = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 0, 1) }; Matrix4x4 m4 = new Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); Vector3 t = Matrix3x3.Transform(v1, m); Matrix3x3.Transform(ref t, ref m, out t); Console.WriteLine($"t to preload: {t}"); double time, endTime; const int testIterations = 10000000; time = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Vector3 refAccumulator = new Vector3(); for (int i = 0; i < testIterations; ++i) { Vector3 t1, t2, t3, t4, t5, t6, t7, t8; Matrix3x3.Transform(ref v1, ref m, out t1); Matrix3x3.Transform(ref v2, ref m, out t2); Matrix3x3.Transform(ref v3, ref m, out t3); Matrix3x3.Transform(ref v4, ref m, out t4); Matrix3x3.Transform(ref v5, ref m, out t5); Matrix3x3.Transform(ref v6, ref m, out t6); Matrix3x3.Transform(ref v7, ref m, out t7); Matrix3x3.Transform(ref v8, ref m, out t8); refAccumulator += t1 + t2 + t3 + t4 + t5 + t6 + t7 + t8; } endTime = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Console.WriteLine($"ref time: {endTime - time}, acc: {refAccumulator}"); time = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Vector3 nonrefAccumulator = new Vector3(); for (int i = 0; i < testIterations; ++i) { var t1 = Matrix3x3.Transform(v1, m); var t2 = Matrix3x3.Transform(v2, m); var t3 = Matrix3x3.Transform(v3, m); var t4 = Matrix3x3.Transform(v4, m); var t5 = Matrix3x3.Transform(v5, m); var t6 = Matrix3x3.Transform(v6, m); var t7 = Matrix3x3.Transform(v7, m); var t8 = Matrix3x3.Transform(v8, m); nonrefAccumulator += t1 + t2 + t3 + t4 + t5 + t6 + t7 + t8; } endTime = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Console.WriteLine($"nonref time: {endTime - time}, acc: {nonrefAccumulator}"); time = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Vector3 ref2Accumulator = new Vector3(); for (int i = 0; i < testIterations; ++i) { Vector3 t1, t2, t3, t4, t5, t6, t7, t8; Matrix3x3.Transform2(ref v1, ref m, out t1); Matrix3x3.Transform2(ref v2, ref m, out t2); Matrix3x3.Transform2(ref v3, ref m, out t3); Matrix3x3.Transform2(ref v4, ref m, out t4); Matrix3x3.Transform2(ref v5, ref m, out t5); Matrix3x3.Transform2(ref v6, ref m, out t6); Matrix3x3.Transform2(ref v7, ref m, out t7); Matrix3x3.Transform2(ref v8, ref m, out t8); ref2Accumulator += t1 + t2 + t3 + t4 + t5 + t6 + t7 + t8; } endTime = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Console.WriteLine($"ref2 time: {endTime - time}, acc: {ref2Accumulator}"); time = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Vector3 nonref2Accumulator = new Vector3(); for (int i = 0; i < testIterations; ++i) { var t1 = Matrix3x3.Transform2(v1, m); var t2 = Matrix3x3.Transform2(v2, m); var t3 = Matrix3x3.Transform2(v3, m); var t4 = Matrix3x3.Transform2(v4, m); var t5 = Matrix3x3.Transform2(v5, m); var t6 = Matrix3x3.Transform2(v6, m); var t7 = Matrix3x3.Transform2(v7, m); var t8 = Matrix3x3.Transform2(v8, m); nonref2Accumulator += t1 + t2 + t3 + t4 + t5 + t6 + t7 + t8; } endTime = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Console.WriteLine($"nonref2 time: {endTime - time}, acc: {nonref2Accumulator}"); time = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Vector3 refTransposeAccumulator = new Vector3(); for (int i = 0; i < testIterations; ++i) { Vector3 t1, t2, t3, t4, t5, t6, t7, t8; Matrix3x3.TransformTranspose(ref v1, ref m, out t1); Matrix3x3.TransformTranspose(ref v2, ref m, out t2); Matrix3x3.TransformTranspose(ref v3, ref m, out t3); Matrix3x3.TransformTranspose(ref v4, ref m, out t4); Matrix3x3.TransformTranspose(ref v5, ref m, out t5); Matrix3x3.TransformTranspose(ref v6, ref m, out t6); Matrix3x3.TransformTranspose(ref v7, ref m, out t7); Matrix3x3.TransformTranspose(ref v8, ref m, out t8); refTransposeAccumulator += t1 + t2 + t3 + t4 + t5 + t6 + t7 + t8; } endTime = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Console.WriteLine($"refTranspose time: {endTime - time}, acc: {refTransposeAccumulator}"); time = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Vector3 nonrefTransposeAccumulator = new Vector3(); for (int i = 0; i < testIterations; ++i) { var t1 = Matrix3x3.TransformTranspose(v1, m); var t2 = Matrix3x3.TransformTranspose(v2, m); var t3 = Matrix3x3.TransformTranspose(v3, m); var t4 = Matrix3x3.TransformTranspose(v4, m); var t5 = Matrix3x3.TransformTranspose(v5, m); var t6 = Matrix3x3.TransformTranspose(v6, m); var t7 = Matrix3x3.TransformTranspose(v7, m); var t8 = Matrix3x3.TransformTranspose(v8, m); nonrefTransposeAccumulator += t1 + t2 + t3 + t4 + t5 + t6 + t7 + t8; } endTime = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Console.WriteLine($"nonrefTranspose time: {endTime - time}, acc: {nonrefTransposeAccumulator}"); time = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Vector3 netAccumulator = new Vector3(); for (int i = 0; i < testIterations; ++i) { var t1 = Vector3.TransformNormal(v1, m4); var t2 = Vector3.TransformNormal(v2, m4); var t3 = Vector3.TransformNormal(v3, m4); var t4 = Vector3.TransformNormal(v4, m4); var t5 = Vector3.TransformNormal(v5, m4); var t6 = Vector3.TransformNormal(v6, m4); var t7 = Vector3.TransformNormal(v7, m4); var t8 = Vector3.TransformNormal(v8, m4); netAccumulator += t1 + t2 + t3 + t4 + t5 + t6 + t7 + t8; } endTime = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Console.WriteLine($"net time: {endTime - time}, acc: {netAccumulator}"); time = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; var v1b = new BEPUVector3(0, 0, 1); var v2b = new BEPUVector3(0, 0, 2); var v3b = new BEPUVector3(0, 0, 3); var v4b = new BEPUVector3(0, 0, 4); var v5b = new BEPUVector3(0, 0, 5); var v6b = new BEPUVector3(0, 0, 6); var v7b = new BEPUVector3(0, 0, 7); var v8b = new BEPUVector3(0, 0, 8); BEPUMatrix3x3 mb = new BEPUMatrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1); BEPUVector3 bepuAccumulator = new BEPUVector3(); for (int i = 0; i < testIterations; ++i) { BEPUVector3 t1, t2, t3, t4, t5, t6, t7, t8; BEPUMatrix3x3.Transform(ref v1b, ref mb, out t1); BEPUMatrix3x3.Transform(ref v2b, ref mb, out t2); BEPUMatrix3x3.Transform(ref v3b, ref mb, out t3); BEPUMatrix3x3.Transform(ref v4b, ref mb, out t4); BEPUMatrix3x3.Transform(ref v5b, ref mb, out t5); BEPUMatrix3x3.Transform(ref v6b, ref mb, out t6); BEPUMatrix3x3.Transform(ref v7b, ref mb, out t7); BEPUMatrix3x3.Transform(ref v8b, ref mb, out t8); BEPUVector3.Add(ref bepuAccumulator, ref t1, out bepuAccumulator); BEPUVector3.Add(ref bepuAccumulator, ref t2, out bepuAccumulator); BEPUVector3.Add(ref bepuAccumulator, ref t3, out bepuAccumulator); BEPUVector3.Add(ref bepuAccumulator, ref t4, out bepuAccumulator); BEPUVector3.Add(ref bepuAccumulator, ref t5, out bepuAccumulator); BEPUVector3.Add(ref bepuAccumulator, ref t6, out bepuAccumulator); BEPUVector3.Add(ref bepuAccumulator, ref t7, out bepuAccumulator); BEPUVector3.Add(ref bepuAccumulator, ref t8, out bepuAccumulator); } endTime = (double)Stopwatch.GetTimestamp() / Stopwatch.Frequency; Console.WriteLine($"bepu time: {endTime - time}, acc: {bepuAccumulator}"); }