private void PreProcessCollisionAccumulated(CollisionInfo collision, float dt)
{
Body body0 = collision.SkinInfo.Skin0.Owner;
Body body1 = collision.SkinInfo.Skin1.Owner;
// make as not satisfied
collision.Satisfied = false;
// always calc the following
Vector3 N = collision.DirToBody0;
float timescale = numPenetrationRelaxtionTimesteps * dt;
for (int pos = 0; pos < collision.NumCollPts; ++pos)
{
CollPointInfo ptInfo = collision.PointInfo[pos];
// some things we only calculate if there are bodies, and they are
// movable
if (body0.Immovable)
ptInfo.Denominator = 0.0f;
else
{
#region INLINE: ptInfo.Denominator = body0.InvMass + Vector3.Dot(N, Vector3.Cross(body0.WorldInvInertia * (Vector3.Cross(ptInfo.R0, N)), ptInfo.R0));
float num0 = ptInfo.Info.R0.Y * N.Z - ptInfo.Info.R0.Z * N.Y;
float num1 = ptInfo.Info.R0.Z * N.X - ptInfo.Info.R0.X * N.Z;
float num2 = ptInfo.Info.R0.X * N.Y - ptInfo.Info.R0.Y * N.X;
float num3 = (((num0 * body0.worldInvInertia.M11) + (num1 * body0.worldInvInertia.M21)) + (num2 * body0.worldInvInertia.M31));
float num4 = (((num0 * body0.worldInvInertia.M12) + (num1 * body0.worldInvInertia.M22)) + (num2 * body0.worldInvInertia.M32));
float num5 = (((num0 * body0.worldInvInertia.M13) + (num1 * body0.worldInvInertia.M23)) + (num2 * body0.worldInvInertia.M33));
num0 = num4 * ptInfo.Info.R0.Z - num5 * ptInfo.Info.R0.Y;
num1 = num5 * ptInfo.Info.R0.X - num3 * ptInfo.Info.R0.Z;
num2 = num3 * ptInfo.Info.R0.Y - num4 * ptInfo.Info.R0.X;
ptInfo.Denominator = body0.InverseMass + ((num0 * N.X) + (num1 * N.Y) + (num2 * N.Z));
#endregion
}
if ((body1 != null) && !body1.Immovable)
{
#region INLINE: ptInfo.Denominator += body1.InvMass + Vector3.Dot(N, Vector3.Cross(body1.WorldInvInertia * (Vector3.Cross(ptInfo.R1, N)), ptInf1.R0));
float num0 = ptInfo.Info.R1.Y * N.Z - ptInfo.Info.R1.Z * N.Y;
float num1 = ptInfo.Info.R1.Z * N.X - ptInfo.Info.R1.X * N.Z;
float num2 = ptInfo.Info.R1.X * N.Y - ptInfo.Info.R1.Y * N.X;
float num3 = (((num0 * body1.worldInvInertia.M11) + (num1 * body1.worldInvInertia.M21)) + (num2 * body1.worldInvInertia.M31));
float num4 = (((num0 * body1.worldInvInertia.M12) + (num1 * body1.worldInvInertia.M22)) + (num2 * body1.worldInvInertia.M32));
float num5 = (((num0 * body1.worldInvInertia.M13) + (num1 * body1.worldInvInertia.M23)) + (num2 * body1.worldInvInertia.M33));
num0 = num4 * ptInfo.Info.R1.Z - num5 * ptInfo.Info.R1.Y;
num1 = num5 * ptInfo.Info.R1.X - num3 * ptInfo.Info.R1.Z;
num2 = num3 * ptInfo.Info.R1.Y - num4 * ptInfo.Info.R1.X;
ptInfo.Denominator += body1.InverseMass + ((num0 * N.X) + (num1 * N.Y) + (num2 * N.Z));
#endregion
}
if (ptInfo.Denominator < JiggleMath.Epsilon)
ptInfo.Denominator = JiggleMath.Epsilon;
// calculate the world position
Vector3.Add(ref body0.oldTransform.Position, ref ptInfo.Info.R0, out ptInfo.Position);
//ptInfo.Position = body0.OldPosition + ptInfo.R0;
// per-point penetetration resolution
if (ptInfo.Info.InitialPenetration > allowedPenetration)
{
ptInfo.MinSeparationVel = (ptInfo.Info.InitialPenetration - allowedPenetration) / timescale;
}
else
{
float approachScale = -0.1f * (ptInfo.Info.InitialPenetration - allowedPenetration) / (JiggleMath.Epsilon + allowedPenetration);
approachScale = MathHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f);
ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / MathHelper.Max(dt, JiggleMath.Epsilon);
}
ptInfo.AccumulatedNormalImpulse = 0.0f;
ptInfo.AccumulatedNormalImpulseAux = 0.0f;
ptInfo.AccumulatedFrictionImpulse = Vector3.Zero;
/// todo take this value from config or derive from the geometry (but don't reference the body in the cache as it
/// may be deleted)
float minDist = 0.2f;
float bestDistSq = minDist * minDist;
Contact.BodyPair bp = new Contact.BodyPair(body0, body1);
int count = catchedContacts.Count;
for (int i = 0; i < count; i++)
{
if (!(bp.BodyA == catchedContacts[i].Pair.BodyA && bp.BodyB == catchedContacts[i].Pair.BodyB))
continue;
//float distSq = (catchedContacts[i].Pair.BodyA == collision.SkinInfo.Skin0.Owner) ?
// Distance.PointPointDistanceSq(catchedContacts[i].Pair.RA, ptInfo.R0) :
// Distance.PointPointDistanceSq(catchedContacts[i].Pair.RA, ptInfo.R1);
float distSq;
if (catchedContacts[i].Pair.BodyA == collision.SkinInfo.Skin0.Owner)
{
float num3 = catchedContacts[i].Pair.RA.X - ptInfo.Info.R0.X;
float num2 = catchedContacts[i].Pair.RA.Y - ptInfo.Info.R0.Y;
float num0 = catchedContacts[i].Pair.RA.Z - ptInfo.Info.R0.Z;
distSq = ((num3 * num3) + (num2 * num2)) + (num0 * num0);
}
//Distance.PointPointDistanceSq(ref catchedContacts[i].Pair.RA, ref ptInfo.R0, out distSq);
else
{
//Distance.PointPointDistanceSq(ref catchedContacts[i].Pair.RA, ref ptInfo.R1, out distSq);
float num3 = catchedContacts[i].Pair.RA.X - ptInfo.Info.R1.X;
float num2 = catchedContacts[i].Pair.RA.Y - ptInfo.Info.R1.Y;
float num0 = catchedContacts[i].Pair.RA.Z - ptInfo.Info.R1.Z;
distSq = ((num3 * num3) + (num2 * num2)) + (num0 * num0);
}
if (distSq < bestDistSq)
{
bestDistSq = distSq;
ptInfo.AccumulatedNormalImpulse = catchedContacts[i].Impulse.NormalImpulse;
ptInfo.AccumulatedNormalImpulseAux = catchedContacts[i].Impulse.NormalImpulseAux;
ptInfo.AccumulatedFrictionImpulse = catchedContacts[i].Impulse.FrictionImpulse;
if (catchedContacts[i].Pair.BodyA != collision.SkinInfo.Skin0.Owner)
ptInfo.AccumulatedFrictionImpulse *= -1;
}
}
//float oldScale = 1.0f;
//ptInfo.AccumulatedNormalImpulse *= oldScale;
//ptInfo.AccumulatedFrictionImpulse *= oldScale;
//ptInfo.AccumulatedNormalImpulseAux *= oldScale;
if (ptInfo.AccumulatedNormalImpulse != 0.0f)
{
//Vector3 impulse = N * ptInfo.AccumulatedNormalImpulse;
Vector3 impulse;
Vector3.Multiply(ref N, ptInfo.AccumulatedNormalImpulse, out impulse);
//impulse += ptInfo.AccumulatedFrictionImpulse;
Vector3.Add(ref impulse, ref ptInfo.AccumulatedFrictionImpulse, out impulse);
body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1);
}
if (ptInfo.AccumulatedNormalImpulseAux != 0.0f)
{
//Vector3 impulse = N * ptInfo.AccumulatedNormalImpulseAux;
Vector3 impulse;
Vector3.Multiply(ref N, ptInfo.AccumulatedNormalImpulseAux, out impulse);
body0.ApplyBodyWorldImpulseAux(ref impulse,ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulseAux(ref impulse,ref ptInfo.Info.R1);
}
}
}
/// <summary>
/// Return this info to the pool.
/// </summary>
/// <param name="info"></param>
public static void FreeCollisionInfo(CollisionInfo info)
{
info.Destroy();
freeInfos.Push(info);
}
private bool ProcessCollisionsForShock(CollisionInfo collision, float dt)
{
collision.Satisfied = true;
Vector3 N = collision.DirToBody0;
// Changed here. N.X = N.Y = 0.0f;
N.X = N.Z = 0.0f;
JiggleMath.NormalizeSafe(ref N);
int iterations = 5;
int pos;
float timescale = penetrationShockRelaxtionTimestep * dt;
for (pos = 0; pos < collision.NumCollPts; ++pos)
{
CollPointInfo ptInfo = collision.PointInfo[pos];
}
// since this is shock, body 0 OR body1 can be immovable. Also, if
// immovable make the constraint against a non-moving object
Body body0 = collision.SkinInfo.Skin0.Owner;
Body body1 = collision.SkinInfo.Skin1.Owner;
if (body0.Immovable)
body0 = null;
if ((body1 != null) && body1.Immovable)
body1 = null;
if (body0 == null && body1 == null)
return false;
for (int iteration = 0; iteration < iterations; ++iteration)
{
for (pos = 0; pos < collision.NumCollPts; ++pos)
{
CollPointInfo ptInfo = collision.PointInfo[pos];
float normalVel = 0.0f;
if (body0 != null)
normalVel = Vector3.Dot(body0.GetVelocity(ptInfo.Info.R0), N) + Vector3.Dot(body0.GetVelocityAux(ptInfo.Info.R0), N);
if (body1 != null)
normalVel -= Vector3.Dot(body1.GetVelocity(ptInfo.Info.R1), N) + Vector3.Dot(body1.GetVelocityAux(ptInfo.Info.R1), N);
float finalNormalVel = (ptInfo.Info.InitialPenetration - allowedPenetration) / timescale;
if (finalNormalVel < 0.0f)
continue;
float impulse = (finalNormalVel - normalVel) / ptInfo.Denominator;
float orig = ptInfo.AccumulatedNormalImpulseAux;
ptInfo.AccumulatedNormalImpulseAux = System.Math.Max(ptInfo.AccumulatedNormalImpulseAux + impulse, 0.0f);
Vector3 actualImpulse = (ptInfo.AccumulatedNormalImpulseAux - orig) * N;
if (body0 != null)
body0.ApplyBodyWorldImpulseAux(ref actualImpulse,ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulseAux(ref actualImpulse,ref ptInfo.Info.R1);
}
}
if (body0 != null)
body0.SetConstraintsAndCollisionsUnsatisfied();
if (body1 != null)
body1.SetConstraintsAndCollisionsUnsatisfied();
return true;
}
private void PreProcessCollision(CollisionInfo collision, float dt)
{
Body body0 = collision.SkinInfo.Skin0.Owner;
Body body1 = collision.SkinInfo.Skin1.Owner;
// make as not satisfied
collision.Satisfied = false;
//always calc the following
Vector3 N = collision.DirToBody0;
float timescale = numPenetrationRelaxtionTimesteps * dt;
for (int pos = 0; pos < collision.NumCollPts; ++pos)
{
CollPointInfo ptInfo = collision.PointInfo[pos];
// some things we only calculate if there are bodies, and they are
// movable
if (body0.Immovable)
ptInfo.Denominator = 0.0f;
else
{
#region INLINE: ptInfo.Denominator = body0.InvMass + Vector3.Dot(N, Vector3.Cross(body0.WorldInvInertia * (Vector3.Cross(ptInfo.R0, N)), ptInfo.R0));
float num0 = (ptInfo.Info.R0.Y * N.Z) - (ptInfo.Info.R0.Z * N.Y);
float num1 = (ptInfo.Info.R0.Z * N.X) - (ptInfo.Info.R0.X * N.Z);
float num2 = (ptInfo.Info.R0.X * N.Y) - (ptInfo.Info.R0.Y * N.X);
float num3 = (((num0 * body0.worldInvInertia.M11) + (num1 * body0.worldInvInertia.M21)) + (num2 * body0.worldInvInertia.M31));
float num4 = (((num0 * body0.worldInvInertia.M12) + (num1 * body0.worldInvInertia.M22)) + (num2 * body0.worldInvInertia.M32));
float num5 = (((num0 * body0.worldInvInertia.M13) + (num1 * body0.worldInvInertia.M23)) + (num2 * body0.worldInvInertia.M33));
num0 = (num4 * ptInfo.Info.R0.Z) - (num5 * ptInfo.Info.R0.Y);
num1 = (num5 * ptInfo.Info.R0.X) - (num3 * ptInfo.Info.R0.Z);
num2 = (num3 * ptInfo.Info.R0.Y) - (num4 * ptInfo.Info.R0.X);
ptInfo.Denominator = body0.InverseMass + ((num0 * N.X) + (num1 * N.Y) + (num2 * N.Z));
#endregion
}
if ((body1 != null) && !body1.Immovable)
{
#region INLINE: ptInfo.Denominator += body1.InvMass + Vector3.Dot(N, Vector3.Cross(body1.WorldInvInertia * (Vector3.Cross(ptInfo.R1, N)), ptInf1.R0));
float num0 = (ptInfo.Info.R1.Y * N.Z) - (ptInfo.Info.R1.Z * N.Y);
float num1 = (ptInfo.Info.R1.Z * N.X) - (ptInfo.Info.R1.X * N.Z);
float num2 = (ptInfo.Info.R1.X * N.Y) - (ptInfo.Info.R1.Y * N.X);
float num3 = (((num0 * body1.worldInvInertia.M11) + (num1 * body1.worldInvInertia.M21)) + (num2 * body1.worldInvInertia.M31));
float num4 = (((num0 * body1.worldInvInertia.M12) + (num1 * body1.worldInvInertia.M22)) + (num2 * body1.worldInvInertia.M32));
float num5 = (((num0 * body1.worldInvInertia.M13) + (num1 * body1.worldInvInertia.M23)) + (num2 * body1.worldInvInertia.M33));
num0 = (num4 * ptInfo.Info.R1.Z) - (num5 * ptInfo.Info.R1.Y);
num1 = (num5 * ptInfo.Info.R1.X) - (num3 * ptInfo.Info.R1.Z);
num2 = (num3 * ptInfo.Info.R1.Y) - (num4 * ptInfo.Info.R1.X);
ptInfo.Denominator += body1.InverseMass + ((num0 * N.X) + (num1 * N.Y) + (num2 * N.Z));
#endregion
}
if (ptInfo.Denominator < JiggleMath.Epsilon)
ptInfo.Denominator = JiggleMath.Epsilon;
// calculate the world position
//Vector3.Add(ref body0.oldTransform.Position, ref ptInfo.R0, out ptInfo.Position);
//ptInfo.Position = body0.OldPosition + ptInfo.R0;
Vector3.Add(ref body0.oldTransform.Position, ref ptInfo.Info.R0, out ptInfo.Position);
// per-point penetration resolution
if (ptInfo.Info.InitialPenetration > allowedPenetration)
{
ptInfo.MinSeparationVel = (ptInfo.Info.InitialPenetration - allowedPenetration) / timescale;
}
else
{
float approachScale = -0.1f * (ptInfo.Info.InitialPenetration - allowedPenetration) / (JiggleMath.Epsilon + allowedPenetration);
approachScale = MathHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f);
ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / MathHelper.Max(dt, JiggleMath.Epsilon);
}
if (ptInfo.MinSeparationVel > maxVelMag)
ptInfo.MinSeparationVel = maxVelMag;
}
}
private bool ProcessCollisionFast(CollisionInfo collision, float dt, bool firstContact)
{
collision.Satisfied = true;
Body body0 = collision.SkinInfo.Skin0.Owner;
Body body1 = collision.SkinInfo.Skin1.Owner;
Vector3 N = collision.DirToBody0;
bool gotOne = false;
for (int pos = collision.NumCollPts; pos-- != 0; )
{
CollPointInfo ptInfo = collision.PointInfo[pos];
float normalVel;
if (body1 != null)
{
//normalVel = Vector3.Dot(body0.GetVelocity(ptInfo.R0) - body1.GetVelocity(ptInfo.R1), collision.DirToBody0);
Vector3 v0, v1;
body0.GetVelocity(ref ptInfo.Info.R0, out v0);
body1.GetVelocity(ref ptInfo.Info.R1, out v1);
Vector3.Subtract(ref v0, ref v1, out v0);
normalVel = Vector3.Dot(v0, N);
}
else
{
Vector3 v0;
body0.GetVelocity(ref ptInfo.Info.R0, out v0);
normalVel = Vector3.Dot(v0, N);
}
if (normalVel > ptInfo.MinSeparationVel)
continue;
float finalNormalVel = -collision.MatPairProperties.Restitution * normalVel;
if (finalNormalVel < minVelForProcessing)
{
// could be zero elasticity in collision, or could be zero
// elasticity in contact - don't care. relax towards 0
// penetration
finalNormalVel = ptInfo.MinSeparationVel;
}
float deltaVel = finalNormalVel - normalVel;
if (deltaVel < minVelForProcessing)
continue;
float normalImpulse = deltaVel / ptInfo.Denominator;
// prepare our return value
gotOne = true;
Vector3 impulse = normalImpulse * N;
body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1);
// recalculate the velocity since it's changed.
Vector3 vrNew = body0.GetVelocity(ptInfo.Info.R0);
if (body1 != null)
vrNew -= body1.GetVelocity(ptInfo.Info.R1);
Vector3 tangentVel = vrNew - Vector3.Dot(vrNew, N) * N;
float tangentSpeed = tangentVel.Length();
if (tangentSpeed > minVelForProcessing)
{
Vector3 T = -tangentVel / tangentSpeed;
// calculate the "inelastic collision" to zeor the relative vel
float denominator = 0.0f;
if (!body0.Immovable)
{
//denominator = body0.InvMass +
// Vector3.Dot(T, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.R0, T), body0.WorldInvInertia), ptInfo.R0));
Vector3 v1; float f2;
Vector3.Cross(ref ptInfo.Info.R0, ref T, out v1);
Vector3.TransformCoordinate(ref v1, ref body0.worldInvInertia, out v1);
Vector3.Cross(ref v1, ref ptInfo.Info.R0, out v1);
f2 = Vector3.Dot(T, v1);
denominator = body0.InverseMass + f2;
}
if ((body1 != null) && (!body1.Immovable))
{
//denominator += body1.InvMass +
// Vector3.Dot(T, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.R1, T), body1.WorldInvInertia), ptInfo.R1));
Vector3 v1; float f2;
Vector3.Cross(ref ptInfo.Info.R1, ref T, out v1);
Vector3.TransformCoordinate(ref v1, ref body1.worldInvInertia, out v1);
Vector3.Cross(ref v1, ref ptInfo.Info.R1, out v1);
f2 = Vector3.Dot(T, v1);
denominator += body1.InverseMass + f2;
}
if (denominator > JiggleMath.Epsilon)
{
float impulseToReverse = tangentSpeed / denominator;
T *= impulseToReverse;
body0.ApplyBodyWorldImpulse(ref T, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref T, ref ptInfo.Info.R1);
}
} // end of friction
}
if (gotOne)
{
body0.SetConstraintsAndCollisionsUnsatisfied();
if (body1 != null)
body1.SetConstraintsAndCollisionsUnsatisfied();
}
return gotOne;
}
private unsafe bool ProcessCollisionCombined(CollisionInfo collision, float dt, bool firstContact)
{
collision.Satisfied = true;
Body body0 = collision.SkinInfo.Skin0.Owner;
Body body1 = collision.SkinInfo.Skin1.Owner;
Vector3 N = collision.DirToBody0;
// the individual impulses in the same order as
// collision->mPointInfo - for friction
float totalImpulse = 0.0f;
int pos;
Vector3 avPos = Vector3.Zero;
float avMinSeparationVel = 0.0f;
// the fastest possible way to allocate short living arrays of primitive types
float* impulses = stackalloc float[CollisionInfo.MaxCollisionPoints];
for (pos = collision.NumCollPts; pos-- != 0; )
{
CollPointInfo ptInfo = collision.PointInfo[pos];
impulses[pos] = 0.0f;
float normalVel;
if (body1 != null)
normalVel = Vector3.Dot(body0.GetVelocity(ptInfo.Info.R0) -
body1.GetVelocity(ptInfo.Info.R1), N);
else
normalVel = Vector3.Dot(body0.GetVelocity(ptInfo.Info.R0), N);
if (normalVel > ptInfo.MinSeparationVel)
continue;
float finalNormalVel = -collision.MatPairProperties.Restitution * normalVel;
if (finalNormalVel < minVelForProcessing)
{
// could be zero elasticity in collision, or could be zero
// elasticity in contact - don't care. relax towards 0
// penetration
finalNormalVel = ptInfo.MinSeparationVel;
}
float deltaVel = finalNormalVel - normalVel;
if (deltaVel < minVelForProcessing)
continue;
float normalImpulse = deltaVel / ptInfo.Denominator;
impulses[pos] = normalImpulse;
totalImpulse += normalImpulse;
avPos = avPos + normalImpulse * ptInfo.Position;
avMinSeparationVel += ptInfo.MinSeparationVel * normalImpulse;
}
if (totalImpulse <= JiggleMath.Epsilon)
return false;
float scale = 1.0f / totalImpulse;
// apply all these impulses (as well as subsequently applying an
// impulse at an averaged position)
for (pos = collision.NumCollPts; pos-- != 0; )
{
if (impulses[pos] > JiggleMath.Epsilon)
{
CollPointInfo ptInfo = collision.PointInfo[pos];
float sc = impulses[pos] * scale;
Vector3 impulse;
Vector3.Multiply(ref N, impulses[pos] * sc, out impulse);
body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1);
}
}
Vector3.Multiply(ref avPos, scale, out avPos);
avMinSeparationVel *= scale;
// now calculate the single impulse to be applied at avPos
Vector3 R0, R1 = Vector3.Zero;
R0 = avPos - body0.Position;
Vector3 Vr = body0.GetVelocity(R0);
if (body1 != null)
{
R1 = avPos - body1.Position;
Vr -= body1.GetVelocity(R1);
}
float normalVel2 = Vector3.Dot(Vr, N);
float normalImpulse2 = 0.0f;
if (normalVel2 < avMinSeparationVel)
{
// coefficient of restitution
float finalNormalVel = -collision.MatPairProperties.Restitution * normalVel2;
if (finalNormalVel < minVelForProcessing)
{
// must be a contact - could be zero elasticity in collision, or
// could be zero elasticity in contact - don't care. relax
// towards 0 penetration
finalNormalVel = avMinSeparationVel;
}
float deltaVel = finalNormalVel - normalVel2;
if (deltaVel > minVelForProcessing)
{
float denominator = 0.0f;
if (!body0.Immovable)
denominator = body0.InverseMass +
Vector3.Dot(N, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(R0, N), body0.WorldInvInertia), R0));
if ((body1 != null) && (!body1.Immovable))
denominator += body1.InverseMass +
Vector3.Dot(N, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(R1, N), body1.WorldInvInertia), R1));
if (denominator < JiggleMath.Epsilon)
denominator = JiggleMath.Epsilon;
normalImpulse2 = deltaVel / denominator;
Vector3 impulse = normalImpulse2 * N;
body0.ApplyWorldImpulse(impulse, avPos);
if (body1 != null)
body1.ApplyNegativeWorldImpulse(impulse, avPos);
}
}
// Now do friction point by point
for (pos = collision.NumCollPts; pos-- != 0; )
{
// For friction, work out the impulse in the opposite direction to
// the tangential velocity that would be required to bring this
// point to a halt. Apply the minimum of this impulse magnitude,
// and the one obtained from the normal impulse. This prevents
// reversing the velocity direction.
//
// However, recalculate the velocity since it's changed.
CollPointInfo ptInfo = collision.PointInfo[pos];
Vector3 vrNew = (body1 != null) ?
(body0.GetVelocity(ptInfo.Info.R0) - body1.GetVelocity(ptInfo.Info.R1)) :
(body0.GetVelocity(ptInfo.Info.R0));
Vector3 T = vrNew - Vector3.Dot(vrNew, N) * N;
float tangentSpeed = T.Length();
if (tangentSpeed > minVelForProcessing)
{
T /= -tangentSpeed;
float sc = impulses[pos] * scale;
float ptNormalImpulse = sc * (normalImpulse2 + impulses[pos]);
// calculate an "inelastic collision" to zero the relative vel
float denominator = 0.0f;
if (!body0.Immovable)
{
denominator = body0.InverseMass +
Vector3.Dot(T, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.Info.R0, T), body0.WorldInvInertia), ptInfo.Info.R0));
}
if ((body1 != null) && (!body1.Immovable))
{
denominator += body1.InverseMass +
Vector3.Dot(T, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.Info.R1, T), body1.WorldInvInertia), ptInfo.Info.R1));
}
if (denominator > JiggleMath.Epsilon)
{
float impulseToReverse = tangentSpeed / denominator;
float impulseFromNormalImpulse =
collision.MatPairProperties.StaticFriction * ptNormalImpulse;
float frictionImpulse;
if (impulseToReverse < impulseFromNormalImpulse)
frictionImpulse = impulseToReverse;
else
frictionImpulse = collision.MatPairProperties.DynamicFriction * ptNormalImpulse;
T *= frictionImpulse;
body0.ApplyBodyWorldImpulse(T, ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref T, ref ptInfo.Info.R1);
}
}
} // end of friction
body0.SetConstraintsAndCollisionsUnsatisfied();
if (body1 != null)
body1.SetConstraintsAndCollisionsUnsatisfied();
return true;
}
private bool ProcessCollisionAccumulated(CollisionInfo collision, float dt, bool firstContact)
{
collision.Satisfied = true;
Body body0 = collision.SkinInfo.Skin0.Owner;
Body body1 = collision.SkinInfo.Skin1.Owner;
Vector3 N = collision.DirToBody0;
bool gotOne = false;
for (int pos = collision.NumCollPts; pos-- != 0; )
{
CollPointInfo ptInfo = collision.PointInfo[pos];
float normalImpulse;
// first the real impulse
{
float normalVel;
if (body1 != null)
{
Vector3 v0, v1;
body0.GetVelocity(ref ptInfo.Info.R0, out v0);
body1.GetVelocity(ref ptInfo.Info.R1, out v1);
Vector3.Subtract(ref v0, ref v1, out v0);
normalVel = Vector3.Dot(v0, N);
//normalVel = Vector3.Dot(v0, N);
}
else
{
Vector3 v0;
body0.GetVelocity(ref ptInfo.Info.R0, out v0);
//normalVel = Vector3.Dot(v0, N);
normalVel = Vector3.Dot(v0, N);
}
// result in zero...
float deltaVel = -normalVel;
// ...except that the impulse reduction to achieve the desired separation must be done
// here - not with aux - because aux would suck objects together
if (ptInfo.MinSeparationVel < 0.0f)
deltaVel += ptInfo.MinSeparationVel;
if (System.Math.Abs(deltaVel) > minVelForProcessing)
{
normalImpulse = deltaVel / ptInfo.Denominator;
float origAccumulatedNormalImpulse = ptInfo.AccumulatedNormalImpulse;
ptInfo.AccumulatedNormalImpulse = MathHelper.Max(ptInfo.AccumulatedNormalImpulse + normalImpulse, 0.0f);
float actualImpulse = ptInfo.AccumulatedNormalImpulse - origAccumulatedNormalImpulse;
//Vector3 impulse = Vector3.Multiply(N, actualImpulse);
Vector3 impulse;
Vector3.Multiply(ref N, actualImpulse, out impulse);
body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1);
// prepare our return value
gotOne = true;
}
}
// now the correction impulse
bool doCorrection = true;
if (doCorrection)
{
float normalVel;
if (body1 != null)
{
Vector3 v0, v1;
body0.GetVelocityAux(ref ptInfo.Info.R0, out v0);
body1.GetVelocityAux(ref ptInfo.Info.R1, out v1);
Vector3.Subtract(ref v0, ref v1, out v0);
normalVel = Vector3.Dot(v0, N);
//normalVel = Vector3.Dot(v0, N);
//normalVel = Vector3.Dot(body0.GetVelocityAux(ptInfo.R0) - body1.GetVelocityAux(ptInfo.R1), collision.DirToBody0);
}
else
{
Vector3 v0;
body0.GetVelocityAux(ref ptInfo.Info.R0, out v0);
//normalVel = Vector3.Dot(v0, N);
normalVel = Vector3.Dot(v0, N);
}
float deltaVel = -normalVel;
// only try to separate objects
if (ptInfo.MinSeparationVel > 0.0f)
deltaVel += ptInfo.MinSeparationVel;
if (System.Math.Abs(deltaVel) > minVelForProcessing)
{
normalImpulse = deltaVel / ptInfo.Denominator;
float origAccumulatedNormalImpulse = ptInfo.AccumulatedNormalImpulseAux;
ptInfo.AccumulatedNormalImpulseAux = System.Math.Max(ptInfo.AccumulatedNormalImpulseAux + normalImpulse, 0.0f);
float actualImpulse = ptInfo.AccumulatedNormalImpulseAux - origAccumulatedNormalImpulse;
//Vector3 impulse = actualImpulse * collision.DirToBody0;
//Vector3 impulse = Vector3.Multiply(N, actualImpulse);
Vector3 impulse;
Vector3.Multiply(ref N, actualImpulse, out impulse);
body0.ApplyBodyWorldImpulseAux(ref impulse, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulseAux(ref impulse,ref ptInfo.Info.R1);
//prepare our return value
gotOne = true;
}
}
// For friction, work out the impulse in the opposite direction to
// the tangential velocity that would be required to bring this
// point to a halt. Apply the minimum of this impulse magnitude,
// and the one obtained from the normal impulse. This prevents
// reversing the velocity direction.
//
// recalculate the velocity since it's changed.
if (ptInfo.AccumulatedNormalImpulse > 0.0f)
{
Vector3 vrNew = body0.GetVelocity(ptInfo.Info.R0);
if (body1 != null)
{
Vector3 pt1Vel;
body1.GetVelocity(ref ptInfo.Info.R1, out pt1Vel);
Vector3.Subtract(ref vrNew, ref pt1Vel, out vrNew);
//vrNew = Vector3.Subtract(vrNew, body1.GetVelocity(ptInfo.R1));
}
//vrNew -= body1.GetVelocity(ptInfo.R1);
//Vector3 tangentVel = vrNew - Vector3.Dot(vrNew, N) * N;
Vector3 tangentVel; float f1;
f1 = Vector3.Dot(vrNew, N);
Vector3.Multiply(ref N, f1, out tangentVel);
Vector3.Subtract(ref vrNew, ref tangentVel, out tangentVel);
float tangentSpeed = tangentVel.Length();
if (tangentSpeed > minVelForProcessing)
{
//Vector3 T = -tangentVel / tangentSpeed;
Vector3 T;
Vector3.Divide(ref tangentVel, -tangentSpeed, out T);
// calculate an "inelastic collision" to zeor the relative vel
float denominator = 0.0f;
if (!body0.Immovable)
{
#region INLINE: denominator = body0.InvMass + Vector3.Dot(T, Vector3.Cross(body0.WorldInvInertia * (Vector3.Cross(ptInfo.R0, T)), ptInfo.R0));
float num0 = ptInfo.Info.R0.Y * T.Z - ptInfo.Info.R0.Z * T.Y;
float num1 = ptInfo.Info.R0.Z * T.X - ptInfo.Info.R0.X * T.Z;
float num2 = ptInfo.Info.R0.X * T.Y - ptInfo.Info.R0.Y * T.X;
float num3 = (((num0 * body0.worldInvInertia.M11) + (num1 * body0.worldInvInertia.M21)) + (num2 * body0.worldInvInertia.M31));
float num4 = (((num0 * body0.worldInvInertia.M12) + (num1 * body0.worldInvInertia.M22)) + (num2 * body0.worldInvInertia.M32));
float num5 = (((num0 * body0.worldInvInertia.M13) + (num1 * body0.worldInvInertia.M23)) + (num2 * body0.worldInvInertia.M33));
num0 = num4 * ptInfo.Info.R0.Z - num5 * ptInfo.Info.R0.Y;
num1 = num5 * ptInfo.Info.R0.X - num3 * ptInfo.Info.R0.Z;
num2 = num3 * ptInfo.Info.R0.Y - num4 * ptInfo.Info.R0.X;
denominator = body0.InverseMass + ((num0 * T.X) + (num1 * T.Y) + (num2 * T.Z));
#endregion
}
if ((body1 != null) && (!body1.Immovable))
{
#region INLINE: denominator += body1.InvMass + Vector3.Dot(T, Vector3.Cross(body1.WorldInvInertia * (Vector3.Cross(ptInfo.R1, T)), ptInfo.R1));
float num0 = ptInfo.Info.R1.Y * T.Z - ptInfo.Info.R1.Z * T.Y;
float num1 = ptInfo.Info.R1.Z * T.X - ptInfo.Info.R1.X * T.Z;
float num2 = ptInfo.Info.R1.X * T.Y - ptInfo.Info.R1.Y * T.X;
float num3 = (((num0 * body1.worldInvInertia.M11) + (num1 * body1.worldInvInertia.M21)) + (num2 * body1.worldInvInertia.M31));
float num4 = (((num0 * body1.worldInvInertia.M12) + (num1 * body1.worldInvInertia.M22)) + (num2 * body1.worldInvInertia.M32));
float num5 = (((num0 * body1.worldInvInertia.M13) + (num1 * body1.worldInvInertia.M23)) + (num2 * body1.worldInvInertia.M33));
num0 = num4 * ptInfo.Info.R1.Z - num5 * ptInfo.Info.R1.Y;
num1 = num5 * ptInfo.Info.R1.X - num3 * ptInfo.Info.R1.Z;
num2 = num3 * ptInfo.Info.R1.Y - num4 * ptInfo.Info.R1.X;
denominator += body1.InverseMass + ((num0 * T.X) + (num1 * T.Y) + (num2 * T.Z));
#endregion
}
if (denominator > JiggleMath.Epsilon)
{
float impulseToReverse = tangentSpeed / denominator;
//Vector3 frictionImpulseVec = T * impulseToReverse;
Vector3 frictionImpulseVec;
Vector3.Multiply(ref T, impulseToReverse, out frictionImpulseVec);
Vector3 origAccumulatedFrictionImpulse = ptInfo.AccumulatedFrictionImpulse;
//ptInfo.AccumulatedFrictionImpulse += frictionImpulseVec;
Vector3.Add(ref ptInfo.AccumulatedFrictionImpulse, ref frictionImpulseVec, out ptInfo.AccumulatedFrictionImpulse);
float AFIMag = ptInfo.AccumulatedFrictionImpulse.Length();
float maxAllowedAFIMAg = collision.MatPairProperties.StaticFriction * ptInfo.AccumulatedNormalImpulse;
if (AFIMag > JiggleMath.Epsilon && AFIMag > maxAllowedAFIMAg)
{
//ptInfo.AccumulatedFrictionImpulse *= maxAllowedAFIMAg / AFIMag;
Vector3.Multiply(ref ptInfo.AccumulatedFrictionImpulse, maxAllowedAFIMAg / AFIMag,
out ptInfo.AccumulatedFrictionImpulse);
}
//Vector3 actualFrictionImpulse = ptInfo.AccumulatedFrictionImpulse - origAccumulatedFrictionImpulse;
Vector3 actualFrictionImpulse;
Vector3.Subtract(ref ptInfo.AccumulatedFrictionImpulse, ref origAccumulatedFrictionImpulse,
out actualFrictionImpulse);
body0.ApplyBodyWorldImpulse(ref actualFrictionImpulse, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref actualFrictionImpulse, ref ptInfo.Info.R1);
}
} // end of friction
}
}
if (gotOne)
{
body0.SetConstraintsAndCollisionsUnsatisfied();
if (body1 != null)
body1.SetConstraintsAndCollisionsUnsatisfied();
}
return gotOne;
}
private bool ProcessCollision(CollisionInfo collision, float dt, bool firstContact)
{
collision.Satisfied = true;
Body body0 = collision.SkinInfo.Skin0.Owner;
Body body1 = collision.SkinInfo.Skin1.Owner;
Vector3 N = collision.DirToBody0;
bool gotOne = false;
for (int pos = 0; pos < collision.NumCollPts; ++pos)
{
CollPointInfo ptInfo = collision.PointInfo[pos];
float normalVel;
if (body1 != null)
{
Vector3 v0, v1;
body0.GetVelocity(ref ptInfo.Info.R0, out v0);
body1.GetVelocity(ref ptInfo.Info.R1, out v1);
Vector3.Subtract(ref v0, ref v1, out v0);
normalVel = Vector3.Dot(v0, N);
}
else
{
Vector3 v0;
body0.GetVelocity(ref ptInfo.Info.R0, out v0);
normalVel = Vector3.Dot(v0, N);
}
if (normalVel > ptInfo.MinSeparationVel)
continue;
float finalNormalVel = -collision.MatPairProperties.Restitution * normalVel;
if (finalNormalVel < minVelForProcessing)
{
// could be zero elasticity in collision, or could be zero
// elasticity in contact - don't care. relax towards 0
// penetration
finalNormalVel = ptInfo.MinSeparationVel;
}
float deltaVel = finalNormalVel - normalVel;
if (deltaVel <= minVelForProcessing)
continue;
if (ptInfo.Denominator < JiggleMath.Epsilon)
{
ptInfo.Denominator = JiggleMath.Epsilon;
}
float normalImpulse = deltaVel / ptInfo.Denominator;
// prepare our return value
gotOne = true;
Vector3 impulse;
Vector3.Multiply(ref N, normalImpulse, out impulse);
body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1);
// For friction, work out the impulse in the opposite direction to
// the tangential velocity that would be required to bring this
// point to a halt. Apply the minimum of this impulse magnitude,
// and the one obtained from the normal impulse. This prevents
// reversing the velocity direction.
//
// recalculate the velocity since it's changed.
Vector3 vrNew;
body0.GetVelocity(ref ptInfo.Info.R0, out vrNew);
if (body1 != null)
{
Vector3 v1;
body1.GetVelocity(ref ptInfo.Info.R1, out v1);
Vector3.Subtract(ref vrNew, ref v1, out vrNew);
//vrNew -= body1.GetVelocity(ptInfo.R1);
}
//Vector3 tangentVel = vrNew - Vector3.Dot(vrNew, N) * N;
Vector3 tangentVel; float f1;
f1 = Vector3.Dot(vrNew, N);
Vector3.Multiply(ref N, f1, out tangentVel);
Vector3.Subtract(ref vrNew, ref tangentVel, out tangentVel);
float tangentSpeed = tangentVel.Length();
if (tangentSpeed > minVelForProcessing)
{
Vector3 T = -tangentVel / tangentSpeed;
// calculate an "inelastic collision" to zero the relative vel
float denominator = 0.0f;
if (!body0.Immovable)
{
#region INLINE: denominator = body0.InvMass + Vector3.Dot(T, Vector3.Cross(body0.WorldInvInertia * (Vector3.Cross(ptInfo.R0, T)), ptInfo.R0));
float num0 = (ptInfo.Info.R0.Y * T.Z) - (ptInfo.Info.R0.Z * T.Y);
float num1 = (ptInfo.Info.R0.Z * T.X) - (ptInfo.Info.R0.X * T.Z);
float num2 = (ptInfo.Info.R0.X * T.Y) - (ptInfo.Info.R0.Y * T.X);
float num3 = (((num0 * body0.worldInvInertia.M11) + (num1 * body0.worldInvInertia.M21)) + (num2 * body0.worldInvInertia.M31));
float num4 = (((num0 * body0.worldInvInertia.M12) + (num1 * body0.worldInvInertia.M22)) + (num2 * body0.worldInvInertia.M32));
float num5 = (((num0 * body0.worldInvInertia.M13) + (num1 * body0.worldInvInertia.M23)) + (num2 * body0.worldInvInertia.M33));
num0 = (num4 * ptInfo.Info.R0.Z) - (num5 * ptInfo.Info.R0.Y);
num1 = (num5 * ptInfo.Info.R0.X) - (num3 * ptInfo.Info.R0.Z);
num2 = (num3 * ptInfo.Info.R0.Y) - (num4 * ptInfo.Info.R0.X);
denominator = body0.InverseMass + ((num0 * T.X) + (num1 * T.Y) + (num2 * T.Z));
#endregion
}
if ((body1 != null) && (!body1.Immovable))
{
#region INLINE: denominator += body1.InvMass + Vector3.Dot(T, Vector3.Cross(body1.WorldInvInertia * (Vector3.Cross(ptInfo.R1, T)), ptInfo.R1));
float num0 = (ptInfo.Info.R1.Y * T.Z) - (ptInfo.Info.R1.Z * T.Y);
float num1 = (ptInfo.Info.R1.Z * T.X) - (ptInfo.Info.R1.X * T.Z);
float num2 = (ptInfo.Info.R1.X * T.Y) - (ptInfo.Info.R1.Y * T.X);
float num3 = (((num0 * body1.worldInvInertia.M11) + (num1 * body1.worldInvInertia.M21)) + (num2 * body1.worldInvInertia.M31));
float num4 = (((num0 * body1.worldInvInertia.M12) + (num1 * body1.worldInvInertia.M22)) + (num2 * body1.worldInvInertia.M32));
float num5 = (((num0 * body1.worldInvInertia.M13) + (num1 * body1.worldInvInertia.M23)) + (num2 * body1.worldInvInertia.M33));
num0 = (num4 * ptInfo.Info.R1.Z) - (num5 * ptInfo.Info.R1.Y);
num1 = (num5 * ptInfo.Info.R1.X) - (num3 * ptInfo.Info.R1.Z);
num2 = (num3 * ptInfo.Info.R1.Y) - (num4 * ptInfo.Info.R1.X);
denominator += body1.InverseMass + ((num0 * T.X) + (num1 * T.Y) + (num2 * T.Z));
#endregion
}
if (denominator > JiggleMath.Epsilon)
{
float impulseToReserve = tangentSpeed / denominator;
float impulseFromNormalImpulse =
collision.MatPairProperties.StaticFriction * normalImpulse;
float frictionImpulse;
if (impulseToReserve < impulseFromNormalImpulse)
frictionImpulse = impulseToReserve;
else
frictionImpulse = collision.MatPairProperties.DynamicFriction * normalImpulse;
T *= frictionImpulse;
body0.ApplyBodyWorldImpulse(ref T, ref ptInfo.Info.R0);
if (body1 != null)
body1.ApplyNegativeBodyWorldImpulse(ref T, ref ptInfo.Info.R1);
}
} // end of friction
}
if (gotOne)
{
body0.SetConstraintsAndCollisionsUnsatisfied();
if (body1 != null)
body1.SetConstraintsAndCollisionsUnsatisfied();
}
return gotOne;
}
private void PreProcessCollisionFast(CollisionInfo collision, float dt)
{
Body body0 = collision.SkinInfo.Skin0.Owner;
Body body1 = collision.SkinInfo.Skin1.Owner;
// make as not satisfied
collision.Satisfied = false;
// always calc the following
Vector3 N = collision.DirToBody0;
float timescale = numPenetrationRelaxtionTimesteps * dt;
const int keep = 3;
if (collision.NumCollPts > keep)
{
Array.Sort( collision.PointInfo, MoreCollPtPenetration);
collision.NumCollPts = keep;
}
for (int pos = 0; pos < collision.NumCollPts; ++pos)
{
CollPointInfo ptInfo = collision.PointInfo[pos];
// some things we only calculate if there are bodies, and they are
// movable
if (body0.Immovable)
ptInfo.Denominator = 0.0f;
else
{
Vector3 cross; float res;
Vector3.Cross(ref ptInfo.Info.R0, ref N, out cross);
Vector3.TransformCoordinate(ref cross, ref body0.worldInvInertia, out cross);
Vector3.Cross(ref cross, ref ptInfo.Info.R0, out cross);
res = Vector3.Dot(N, cross);
ptInfo.Denominator = body0.InverseMass + res;
}
if ((body1 != null) && !body1.Immovable)
{
//ptInfo.Denominator += body1.InvMass +
// Vector3.Dot(N, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.R1, N), body1.WorldInvInertia), ptInfo.R1));
Vector3 cross; float res;
Vector3.Cross(ref ptInfo.Info.R1, ref N, out cross);
Vector3.TransformCoordinate(ref cross, ref body1.worldInvInertia, out cross);
Vector3.Cross(ref cross, ref ptInfo.Info.R1, out cross);
res = Vector3.Dot(N, cross);
ptInfo.Denominator += body1.InverseMass + res;
}
if (ptInfo.Denominator < JiggleMath.Epsilon)
ptInfo.Denominator = JiggleMath.Epsilon;
// calculate the world position
Vector3.Add(ref body0.oldTransform.Position, ref ptInfo.Info.R0, out ptInfo.Position);
// per-point penetration resolution
if (ptInfo.Info.InitialPenetration > allowedPenetration)
{
ptInfo.MinSeparationVel = (ptInfo.Info.InitialPenetration - allowedPenetration) / timescale;
}
else
{
float approachScale = -0.1f * (ptInfo.Info.InitialPenetration - allowedPenetration) / (JiggleMath.Epsilon + allowedPenetration);
approachScale = MathHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f);
ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / MathHelper.Max(dt, JiggleMath.Epsilon);
}
if (ptInfo.MinSeparationVel > maxVelMag)
ptInfo.MinSeparationVel = maxVelMag;
}
}
