/// <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 } }