public static void Get(out Matrix3x3 m)
{
m = new Matrix3x3();
m.X = new Vector3();
m.Y = new Vector3();
m.Z = new Vector3();
}
public static void Transform2(ref Vector3 v, ref Matrix3x3 m, out Vector3 result)
{
var x = new Vector3(m.X.X, m.Y.X, m.Z.X);
var y = new Vector3(m.X.Y, m.Y.Y, m.Z.Y);
var z = new Vector3(m.X.Z, m.Y.Z, m.Z.Z);
result = new Vector3(Vector3.Dot(v, x), Vector3.Dot(v, y), Vector3.Dot(v, z));
}
public static Vector3 Transform2(Vector3 v, Matrix3x3 m)
{
var x = new Vector3(m.X.X, m.Y.X, m.Z.X);
var y = new Vector3(m.X.Y, m.Y.Y, m.Z.Y);
var z = new Vector3(m.X.Z, m.Y.Z, m.Z.Z);
return new Vector3(Vector3.Dot(v, x), Vector3.Dot(v, y), Vector3.Dot(v, z));
}
public static void Transform(ref Vector3 v, ref Matrix3x3 m, out Vector3 result)
{
var x = new Vector3(v.X);
var y = new Vector3(v.Y);
var z = new Vector3(v.Z);
result = m.X * x + m.Y * y + m.Z * z;
}
public static Vector3 Transform(Vector3 v, Matrix3x3 m)
{
var x = new Vector3(v.X);
var y = new Vector3(v.Y);
var z = new Vector3(v.Z);
return m.X * x + m.Y * y + m.Z * 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)
};
var constraint = new HybridPenetrationConstraint
{
a0 = a,
a1 = a,
a2 = a,
a3 = a,
b0 = b,
b1 = b,
b2 = b,
b3 = 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;
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($"Hybrid: {endtime - startTime}, acc: {constraint.AccumulatedImpulse}");
}
public Matrix3x3Width4(ref Matrix3x3 m1, ref Matrix3x3 m2, ref Matrix3x3 m3, ref Matrix3x3 m4)
{
M11 = new Vector4(m1.X.X, m2.X.X, m3.X.X, m4.X.X);
M12 = new Vector4(m1.X.Y, m2.X.Y, m3.X.Y, m4.X.Y);
M13 = new Vector4(m1.X.Z, m2.X.Z, m3.X.Z, m4.X.Z);
M21 = new Vector4(m1.Y.X, m2.Y.X, m3.Y.X, m4.Y.X);
M22 = new Vector4(m1.Y.Y, m2.Y.Y, m3.Y.Y, m4.Y.Y);
M23 = new Vector4(m1.Y.Z, m2.Y.Z, m3.Y.Z, m4.Y.Z);
M31 = new Vector4(m1.Z.X, m2.Z.X, m3.Z.X, m4.Z.X);
M32 = new Vector4(m1.Z.Y, m2.Z.Y, m3.Z.Y, m4.Z.Y);
M33 = new Vector4(m1.Z.Z, m2.Z.Z, m3.Z.Z, m4.Z.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}");
}
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}");
}
public static void TransformTranspose(ref Vector3 v, ref Matrix3x3 m, out Vector3 result)
{
result = new Vector3(Vector3.Dot(v, m.X), Vector3.Dot(v, m.Y), Vector3.Dot(v, m.Z));
}
public static Vector3 TransformTranspose(Vector3 v, Matrix3x3 m)
{
return new Vector3(Vector3.Dot(v, m.X), Vector3.Dot(v, m.Y), Vector3.Dot(v, m.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;
VectorizedPenetrationConstraint constraint = new VectorizedPenetrationConstraint
{
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) },
ContactPosition = new Vector3Width4(),
ContactNormal = new Vector3Width4(ref up, ref up, ref up, ref up),
ContactPenetration = new Vector4()
};
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 < 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($"Vectorized: {endtime - startTime}, acc: {constraint.AccumulatedImpulse}");
}
public static void TransformTranspose(ref Vector3 v, ref Matrix3x3 m, out Vector3 result)
{
result = new Vector3(Vector3.Dot(v, m.X), Vector3.Dot(v, m.Y), Vector3.Dot(v, m.Z));
}
public static Vector3 TransformTranspose(Vector3 v, Matrix3x3 m)
{
return(new Vector3(Vector3.Dot(v, m.X), Vector3.Dot(v, m.Y), Vector3.Dot(v, m.Z)));
}
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;
VectorizedPenetrationConstraint constraint = new VectorizedPenetrationConstraint
{
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)
},
ContactPosition = new Vector3Width4(),
ContactNormal = new Vector3Width4(ref up, ref up, ref up, ref up),
ContactPenetration = new Vector4()
};
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 < 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($"Vectorized: {endtime - startTime}, acc: {constraint.AccumulatedImpulse}");
}