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) }; var constraint = new SingleVectorizedPenetrationConstraint { ConnectionA = a, ConnectionB = b, ContactPosition = new Vector3(), ContactNormal = new Vector3(0, 1, 0), ContactPenetration = 0 }; float dt = 1 / 60f; float inverseDt = 1 / dt; constraint.Prestep(inverseDt); constraint.WarmStart(); constraint.SolveIteration(); const int testCount = VectorizedConstraintTest.TestCount * 4; const int iterationCount = VectorizedConstraintTest.IterationCount; var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < testCount; ++i) { constraint.Prestep(inverseDt); constraint.WarmStart(); for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) { constraint.SolveIteration(); } } var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; Console.WriteLine($"Single vectorized: {endtime - startTime}, acc: {constraint.AccumulatedImpulse}"); }
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) }; var constraint = new SingleVectorizedPenetrationConstraint { ConnectionA = a, ConnectionB = b, ContactPosition = new Vector3(), ContactNormal = new Vector3(0, 1, 0), ContactPenetration = 0 }; float dt = 1 / 60f; float inverseDt = 1 / dt; constraint.Prestep(inverseDt); constraint.WarmStart(); constraint.SolveIteration(); const int testCount = VectorizedConstraintTest.TestCount * 4; const int iterationCount = VectorizedConstraintTest.IterationCount; var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < testCount; ++i) { constraint.Prestep(inverseDt); constraint.WarmStart(); for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) { constraint.SolveIteration(); } } var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; Console.WriteLine($"Single vectorized: {endtime - startTime}, acc: {constraint.AccumulatedImpulse}"); }