/// <summary>
/// Return this info to the pool.
/// </summary>
/// <param name="info"></param>
public static void FreeCollisionInfo(CollisionInfo info)
{
info.Destroy();
freeInfos.Push(info);
}
/// <summary>
/// Return this info to the pool.
/// </summary>
/// <param name="info"></param>
public static void FreeCollisionInfo(CollisionInfo info)
{
info.Destroy();
freeInfos.Push(info);
}
/// <summary>
/// PreProcessCollision
/// </summary>
/// <param name="collision"></param>
/// <param name="dt"></param>
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 = OpenTKHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f);
ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / OpenTKHelper.Max(dt, JiggleMath.Epsilon);
}
if (ptInfo.MinSeparationVel > maxVelMag)
ptInfo.MinSeparationVel = maxVelMag;
}
}
/// <summary>
/// PreProcessCollisionAccumulated
/// </summary>
/// <param name="collision"></param>
/// <param name="dt"></param>
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 = OpenTKHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f);
ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / OpenTKHelper.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>
/// ProcessCollisionFast
/// </summary>
/// <param name="collision"></param>
/// <param name="dt"></param>
/// <param name="firstContact"></param>
/// <returns>bool</returns>
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.Transform(Vector3.Cross(ptInfo.R0, T), body0.WorldInvInertia), ptInfo.R0));
Vector3 v1; float f2;
Vector3.Cross(ref ptInfo.Info.R0, ref T, out v1);
Vector3Extensions.TransformNormal(ref v1, ref body0.worldInvInertia, out v1);
Vector3.Cross(ref v1, ref ptInfo.Info.R0, out v1);
Vector3.Dot(ref T, ref v1, out f2);
denominator = body0.InverseMass + f2;
}
if ((body1 != null) && (!body1.Immovable))
{
//denominator += body1.InvMass +
// Vector3.Dot(T, Vector3.Cross(Vector3.Transform(Vector3.Cross(ptInfo.R1, T), body1.WorldInvInertia), ptInfo.R1));
Vector3 v1; float f2;
Vector3.Cross(ref ptInfo.Info.R1, ref T, out v1);
Vector3Extensions.TransformNormal(ref v1, ref body1.worldInvInertia, out v1);
Vector3.Cross(ref v1, ref ptInfo.Info.R1, out v1);
Vector3.Dot(ref T, ref v1, out f2);
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;
}
/// <summary>
/// ProcessCollisionsForShock
/// </summary>
/// <param name="collision"></param>
/// <param name="dt"></param>
/// <returns></returns>
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;
}
/// <summary>
/// ProcessCollisionCombined
/// </summary>
/// <param name="collision"></param>
/// <param name="dt"></param>
/// <param name="firstContact"></param>
/// <returns>bool</returns>
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(Vector3Extensions.TransformNormal(Vector3.Cross(R0, N), body0.WorldInvInertia), R0));
if ((body1 != null) && (!body1.Immovable))
denominator += body1.InverseMass +
Vector3.Dot(N, Vector3.Cross(Vector3Extensions.TransformNormal(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(Vector3Extensions.TransformNormal(Vector3.Cross(ptInfo.Info.R0, T), body0.WorldInvInertia), ptInfo.Info.R0));
}
if ((body1 != null) && (!body1.Immovable))
{
denominator += body1.InverseMass +
Vector3.Dot(T, Vector3.Cross(Vector3Extensions.TransformNormal(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;
}
/// <summary>
/// ProcessCollisionAccumulated
/// </summary>
/// <param name="collision"></param>
/// <param name="dt"></param>
/// <param name="firstContact"></param>
/// <returns>bool</returns>
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);
Vector3.Dot(ref v0, ref N, out normalVel);
//normalVel = Vector3.Dot(v0, N);
}
else
{
Vector3 v0;
body0.GetVelocity(ref ptInfo.Info.R0, out v0);
//normalVel = Vector3.Dot(v0, N);
Vector3.Dot(ref v0, ref N, out normalVel);
}
// 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 = OpenTKHelper.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);
Vector3.Dot(ref v0, ref N, out normalVel);
//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);
Vector3.Dot(ref v0, ref N, out normalVel);
}
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;
Vector3.Dot(ref vrNew, ref N, out f1);
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;
}
/// <summary>
/// ProcessCollision
/// </summary>
/// <param name="collision"></param>
/// <param name="dt"></param>
/// <param name="firstContact"></param>
/// <returns>bool</returns>
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);
Vector3.Dot(ref v0, ref N, out normalVel);
}
else
{
Vector3 v0;
body0.GetVelocity(ref ptInfo.Info.R0, out v0);
Vector3.Dot(ref v0, ref N, out normalVel);
}
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;
Vector3.Dot(ref vrNew, ref N, out f1);
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;
}
/// <summary>
/// PreProcessCollisionFast
/// </summary>
/// <param name="collision"></param>
/// <param name="dt"></param>
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);
Vector3Extensions.TransformNormal(ref cross, ref body0.worldInvInertia, out cross);
Vector3.Cross(ref cross, ref ptInfo.Info.R0, out cross);
Vector3.Dot(ref N, ref cross, out res);
ptInfo.Denominator = body0.InverseMass + res;
}
if ((body1 != null) && !body1.Immovable)
{
//ptInfo.Denominator += body1.InvMass +
// Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(ptInfo.R1, N), body1.WorldInvInertia), ptInfo.R1));
Vector3 cross; float res;
Vector3.Cross(ref ptInfo.Info.R1, ref N, out cross);
Vector3Extensions.TransformNormal(ref cross, ref body1.worldInvInertia, out cross);
Vector3.Cross(ref cross, ref ptInfo.Info.R1, out cross);
Vector3.Dot(ref N, ref cross, out res);
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 = OpenTKHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f);
ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / OpenTKHelper.Max(dt, JiggleMath.Epsilon);
}
if (ptInfo.MinSeparationVel > maxVelMag)
ptInfo.MinSeparationVel = maxVelMag;
}
}
/// <summary>
/// 指定の方向ベクトルについての衝突情報とその衝突法線ベクトルを探します。
/// </summary>
/// <param name="direction">判定する方向ベクトル。</param>
/// <param name="resultCollision">衝突情報 (JigLibX)。</param>
/// <param name="resultNormal">衝突法線ベクトル。</param>
void FindCollisionAndNormal(ref Vector3 direction, out CollisionInfo resultCollision, out Vector3? resultNormal)
{
resultCollision = null;
resultNormal = null;
if (direction.IsZero()) return;
// 衝突リストから指定された検査方向ベクトルの逆ベクトルを
// 衝突法線ベクトルとする CollisionSkin を探します。
// 逆ベクトルと法線ベクトルの一致性は、
// その内積結果が一定の範囲内である場合に一致とみなします。
var dir = -direction;
dir.Normalize();
foreach (var collision in Collisions)
{
var normal = collision.DirToBody0;
if (collision.SkinInfo.Skin1 == this)
{
Vector3.Negate(ref normal, out normal);
}
float nDotDir;
Vector3.Dot(ref normal, ref dir, out nDotDir);
if (0.7f < nDotDir)
{
resultCollision = collision;
resultNormal = normal;
break;
}
}
}