|
private Transform ( Vector3 v, |
||
| v | Vector3 | |
| m | ||
| 리턴 | Vector3 | |
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}");
}
