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 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}");
}
