/// <summary>
/// SegmentSphereIntersection
/// </summary>
/// <param name="ts"></param>
/// <param name="seg"></param>
/// <param name="sphere"></param>
/// <returns>bool</returns>
public static bool SegmentSphereIntersection(out float ts, Segment seg, Sphere sphere)
{
Vector3 r = seg.Delta;
Vector3 s = seg.Origin - sphere.Position;
float radiusSq = sphere.Radius * sphere.Radius;
float rSq = r.LengthSquared;
ts = float.MaxValue;
if (rSq < radiusSq)
{
// starting inside
ts = 0.0f;
return(false);
}
float sDotr = Vector3.Dot(s, r);
float sSq = s.LengthSquared;
float sigma = (sDotr * sDotr) - rSq * (sSq - radiusSq);
if (sigma < 0.0f)
{
return(false);
}
float sigmaSqrt = (float)System.Math.Sqrt((float)sigma);
float lambda1 = (-sDotr - sigmaSqrt) / rSq;
float lambda2 = (-sDotr + sigmaSqrt) / rSq;
if (lambda1 > 1.0f || lambda2 < 0.0f)
{
return(false);
}
// intersection!
ts = OpenTKHelper.Max(lambda1, 0.0f);
return(true);
}
/// <summary>
/// Apply
/// </summary>
/// <param name="dt"></param>
/// <returns>bool</returns>
public override bool Apply(float dt)
{
this.Satisfied = true;
bool body0FrozenPre = !body0.IsActive;
bool body1FrozenPre = !body1.IsActive;
if (body0FrozenPre && body1FrozenPre)
{
return(false);
}
#region REFERENCE: Vector3 currentVel0 = body0.Velocity + Vector3.Cross(body0.AngVel, R0);
Vector3 currentVel0 = body0.AngularVelocity;
Vector3.Cross(ref currentVel0, ref R0, out currentVel0);
Vector3.Add(ref body0.transformRate.Velocity, ref currentVel0, out currentVel0);
#endregion
#region REFERENCE: Vector3 currentVel1 = body1.Velocity + Vector3.Cross(body1.AngVel, R1);
Vector3 currentVel1 = body1.AngularVelocity;
Vector3.Cross(ref currentVel1, ref R1, out currentVel1);
Vector3.Add(ref body1.transformRate.Velocity, ref currentVel1, out currentVel1);
#endregion
// predict a new location
#region REFERENCE: Vector3 predRelPos0 = (currentRelPos0 + (currentVel0 - currentVel1) * dt);
Vector3 predRelPos0;
Vector3.Subtract(ref currentVel0, ref currentVel1, out predRelPos0);
Vector3.Multiply(ref predRelPos0, dt, out predRelPos0);
Vector3.Add(ref predRelPos0, ref currentRelPos0, out predRelPos0);
#endregion
// if the new position is out of range then clamp it
Vector3 clampedRelPos0 = predRelPos0;
float clampedRelPos0Mag = clampedRelPos0.Length;
if (clampedRelPos0Mag <= JiggleMath.Epsilon)
{
return(false);
}
if (clampedRelPos0Mag > mMaxDistance)
#region REFERENCE: clampedRelPos0 *= mMaxDistance / clampedRelPos0Mag;
{
Vector3.Multiply(ref clampedRelPos0, mMaxDistance / clampedRelPos0Mag, out clampedRelPos0);
}
#endregion
// now claculate desired vel based on the current pos, new/clamped
// pos and dt
#region REFERENCE: Vector3 desiredRelVel0 = ((clampedRelPos0 - currentRelPos0) / System.Math.Max(dt, JiggleMath.Epsilon));
Vector3 desiredRelVel0;
Vector3.Subtract(ref clampedRelPos0, ref currentRelPos0, out desiredRelVel0);
Vector3.Divide(ref desiredRelVel0, OpenTKHelper.Max(dt, JiggleMath.Epsilon), out desiredRelVel0);
#endregion
// Vr is -ve the total velocity change
#region REFERENCE: Vector3 Vr = (currentVel0 - currentVel1) - desiredRelVel0;
Vector3 Vr;
Vector3.Subtract(ref currentVel0, ref currentVel1, out Vr);
Vector3.Subtract(ref Vr, ref desiredRelVel0, out Vr);
#endregion
float normalVel = Vr.Length;
// limit it
if (normalVel > maxVelMag)
{
#region REFERENCE: Vr *= (maxVelMag / normalVel);
Vector3.Multiply(ref Vr, maxVelMag / normalVel, out Vr);
#endregion
normalVel = maxVelMag;
}
else if (normalVel < minVelForProcessing)
{
return(false);
}
#region REFERENCE: Vector3 N = Vr / normalVel;
Vector3 N;
Vector3.Divide(ref Vr, normalVel, out N);
#endregion
#region REFERENCE: float denominator = body0.InvMass + body1.InvMass + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R0, N), body0.WorldInvInertia), R0)) + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R1, N), body1.WorldInvInertia), R1));
Vector3 v1; float f1, f2;
Vector3.Cross(ref R0, ref N, out v1);
Vector3Extensions.TransformNormal(ref v1, ref body0.worldInvInertia, out v1);
Vector3.Cross(ref v1, ref R0, out v1);
Vector3.Dot(ref N, ref v1, out f1);
Vector3.Cross(ref R1, ref N, out v1);
Vector3Extensions.TransformNormal(ref v1, ref body1.worldInvInertia, out v1);
Vector3.Cross(ref v1, ref R1, out v1);
Vector3.Dot(ref N, ref v1, out f2);
float denominator = body0.InverseMass + body1.InverseMass + f1 + f2;
#endregion
if (denominator < JiggleMath.Epsilon)
{
return(false);
}
float normalImpulse = -normalVel / denominator;
#region REFERENCE: if (!body0.Immovable) body0.ApplyWorldImpulse(normalImpulse * N, worldPos);
Vector3 imp;
Vector3.Multiply(ref N, normalImpulse, out imp);
if (!body0.Immovable)
{
body0.ApplyWorldImpulse(ref imp, ref worldPos);
}
#endregion
#region REFERENCE: if (!body1.Immovable) body1.ApplyWorldImpulse(-normalImpulse * N, worldPos);
Vector3.Multiply(ref N, -normalImpulse, out imp);
if (!body1.Immovable)
{
body1.ApplyWorldImpulse(ref imp, ref worldPos);
}
#endregion
body0.SetConstraintsAndCollisionsUnsatisfied();
body1.SetConstraintsAndCollisionsUnsatisfied();
this.Satisfied = true;
return(true);
}
/// <summary>
/// Detect BoxPlane Collisions.
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
Box oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
Box newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
JPlane oldPlane = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as JPlane;
JPlane newPlane = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as JPlane;
Matrix4 newPlaneInvTransform = newPlane.InverseTransformMatrix;
Vector3 newBoxCen = Vector3.Transform(newBox.GetCentre(), newPlaneInvTransform);
// quick check
float centreDist = Distance.PointPlaneDistance(newBoxCen, newPlane);
if (centreDist > collTolerance + newBox.GetBoundingRadiusAroundCentre())
{
return;
}
Matrix4 oldPlaneInvTransform = oldPlane.InverseTransformMatrix;
Vector3[] newPts;
newBox.GetCornerPoints(out newPts);
Vector3[] oldPts;
oldBox.GetCornerPoints(out oldPts);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
for (int i = 0; i < 8; ++i)
{
Vector3.Transform(ref oldPts[i], ref oldPlaneInvTransform, out oldTransPts[i]);
Vector3.Transform(ref newPts[i], ref newPlaneInvTransform, out newPts[i]);
float oldDepth = -Distance.PointPlaneDistance(ref oldTransPts[i], oldPlane);
float newDepth = -Distance.PointPlaneDistance(ref newPts[i], newPlane);
if (OpenTKHelper.Max(oldDepth, newDepth) > -collTolerance)
{
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses instead of reallocating.
collPts[numCollPts].R0 = oldPts[i] - body0Pos;
collPts[numCollPts].R1 = oldPts[i] - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
}
}
if (numCollPts > 0)
{
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPts, numCollPts);
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
