/// <summary> /// CollDetect /// </summary> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Capsule oldCapsule0 = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; Capsule newCapsule0 = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; Segment oldSeg0 = new Segment(oldCapsule0.Position, oldCapsule0.Length * oldCapsule0.Orientation.Backward()); Segment newSeg0 = new Segment(newCapsule0.Position, newCapsule0.Length * newCapsule0.Orientation.Backward()); Capsule oldCapsule1 = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Capsule; Capsule newCapsule1 = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Capsule; Segment oldSeg1 = new Segment(oldCapsule1.Position, oldCapsule1.Length * oldCapsule1.Orientation.Backward()); Segment newSeg1 = new Segment(newCapsule1.Position, newCapsule1.Length * newCapsule1.Orientation.Backward()); float radSum = newCapsule0.Radius + newCapsule1.Radius; float oldt0, oldt1; float newt0, newt1; float oldDistSq = Distance.SegmentSegmentDistanceSq(out oldt0, out oldt1, oldSeg0, oldSeg1); float newDistSq = Distance.SegmentSegmentDistanceSq(out newt0, out newt1, newSeg0, newSeg1); if (System.Math.Min(oldDistSq, newDistSq) < ((radSum + collTolerance) * (radSum + collTolerance))) { Vector3 pos0 = oldSeg0.GetPoint(oldt0); Vector3 pos1 = oldSeg1.GetPoint(oldt1); Vector3 delta = pos0 - pos1; float dist = (float)System.Math.Sqrt((float)oldDistSq); float depth = radSum - dist; if (dist > JiggleMath.Epsilon) { delta /= dist; } else { // todo - make this not random delta = Vector3Extensions.TransformNormal(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360)))); } Vector3 worldPos = pos1 + (oldCapsule1.Radius - 0.5f * depth) * delta; unsafe { SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1); } } }
/// <summary> /// Initialise /// </summary> /// <param name="body0"></param> /// <param name="body1"></param> /// <param name="hingeAxis"></param> /// <param name="hingePosRel0"></param> /// <param name="hingeHalfWidth"></param> /// <param name="hingeFwdAngle"></param> /// <param name="hingeBckAngle"></param> /// <param name="sidewaysSlack"></param> /// <param name="damping"></param> public void Initialise(Body body0, Body body1, Vector3 hingeAxis, Vector3 hingePosRel0, float hingeHalfWidth, float hingeFwdAngle, float hingeBckAngle, float sidewaysSlack, float damping) { this.body0 = body0; this.body1 = body1; this.hingeAxis = hingeAxis; this.hingePosRel0 = hingePosRel0; this.usingLimit = false; this.damping = damping; // tScalar allowedDistance = 0.005f; this.hingeAxis.Normalize(); Vector3 hingePosRel1 = body0.Position + hingePosRel0 - body1.Position; // generate the two positions relative to each body Vector3 relPos0a = hingePosRel0 + hingeHalfWidth * hingeAxis; Vector3 relPos0b = hingePosRel0 - hingeHalfWidth * hingeAxis; Vector3 relPos1a = hingePosRel1 + hingeHalfWidth * hingeAxis; Vector3 relPos1b = hingePosRel1 - hingeHalfWidth * hingeAxis; float timescale = 1.0f / 20.0f; float allowedDistanceMid = 0.005f; float allowedDistanceSide = sidewaysSlack * hingeHalfWidth; mSidePointConstraints = new ConstraintMaxDistance[2]; mSidePointConstraints[0] = new ConstraintMaxDistance(); mSidePointConstraints[1] = new ConstraintMaxDistance(); mSidePointConstraints[0].Initialise(body0, relPos0a, body1, relPos1a, allowedDistanceSide); mSidePointConstraints[1].Initialise(body0, relPos0b, body1, relPos1b, allowedDistanceSide); mMidPointConstraint = new ConstraintPoint(); mMidPointConstraint.Initialise(body0, hingePosRel0, body1, hingePosRel1, allowedDistanceMid, timescale); if (hingeFwdAngle <= 150) // MAX_HINGE_ANGLE_LIMIT { // choose a direction that is perpendicular to the hinge Vector3 perpDir = Vector3Extensions.Up; if (Vector3.Dot(perpDir, hingeAxis) > 0.1f) { perpDir = Vector3Extensions.Right; } // now make it perpendicular to the hinge Vector3 sideAxis = Vector3.Cross(hingeAxis, perpDir); perpDir = Vector3.Cross(sideAxis, hingeAxis); perpDir.Normalize(); // the length of the "arm" TODO take this as a parameter? what's // the effect of changing it? float len = 10.0f * hingeHalfWidth; // Choose a position using that dir. this will be the anchor point // for body 0. relative to hinge Vector3 hingeRelAnchorPos0 = perpDir * len; // anchor point for body 2 is chosen to be in the middle of the // angle range. relative to hinge float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle); Vector3 hingeRelAnchorPos1 = Vector3Extensions.TransformNormal(hingeRelAnchorPos0, Matrix4.CreateFromAxisAngle(hingeAxis, OpenTKHelper.ToRadians(-angleToMiddle))); // work out the "string" length float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle); float allowedDistance = len * 2.0f * (float)System.Math.Sin(OpenTKHelper.ToRadians(hingeHalfAngle * 0.5f)); Vector3 hingePos = body1.Position + hingePosRel0; Vector3 relPos0c = hingePos + hingeRelAnchorPos0 - body0.Position; Vector3 relPos1c = hingePos + hingeRelAnchorPos1 - body1.Position; mMaxDistanceConstraint = new ConstraintMaxDistance(); mMaxDistanceConstraint.Initialise(body0, relPos0c, body1, relPos1c, allowedDistance); usingLimit = true; } if (damping <= 0.0f) { damping = -1.0f; // just make sure that a value of 0.0 doesn't mess up... } else { damping = OpenTKHelper.Clamp(damping, 0, 1); } }
/// <summary> /// CollDetect /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { // get the skins in the order that we're expectiing CollDetectInfo info = infoOrig; 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; // todo - proper swept test Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward()); Segment newSeg = new Segment(newCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward()); float radius = oldCapsule.Radius; Box oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box; Box newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box; float oldSegT; float oldBoxT0, oldBoxT1, oldBoxT2; float oldDistSq = Distance.SegmentBoxDistanceSq(out oldSegT, out oldBoxT0, out oldBoxT1, out oldBoxT2, oldSeg, oldBox); float newSegT; float newBoxT0, newBoxT1, newBoxT2; float newDistSq = Distance.SegmentBoxDistanceSq(out newSegT, out newBoxT0, out newBoxT1, out newBoxT2, newSeg, newBox); if (OpenTKHelper.Min(oldDistSq, newDistSq) < ((radius + collTolerance) * (radius + collTolerance))) { Vector3 segPos = oldSeg.GetPoint(oldSegT); Vector3 boxPos = oldBox.GetCentre() + oldBoxT0 * oldBox.Orientation.Right() + oldBoxT1 * oldBox.Orientation.Up() + oldBoxT2 * oldBox.Orientation.Backward(); float dist = (float)System.Math.Sqrt((float)oldDistSq); float depth = radius - dist; Vector3 dir; if (dist > JiggleMath.Epsilon) { dir = segPos - boxPos; JiggleMath.NormalizeSafe(ref dir); } else if ((segPos - oldBox.GetCentre()).LengthSquared > JiggleMath.Epsilon) { dir = segPos - oldBox.GetCentre(); JiggleMath.NormalizeSafe(ref dir); } else { // todo - make this not random dir = Vector3.Transform(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360)))); } unsafe { SmallCollPointInfo collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1); } } }
/// <summary> /// CollDetect /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { CollDetectInfo info = infoOrig; // get the skins in the order that we're expecting 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; // todo - proper swept test Sphere oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere; Sphere newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere; Capsule oldCapsule = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Capsule; Capsule newCapsule = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Capsule; Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward()); Segment newSeg = new Segment(oldCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward()); float radSum = newCapsule.Radius + newSphere.Radius; float oldt, newt; float oldDistSq = Distance.PointSegmentDistanceSq(out oldt, oldSphere.Position, oldSeg); float newDistSq = Distance.PointSegmentDistanceSq(out newt, newSphere.Position, newSeg); if (OpenTKHelper.Min(oldDistSq, newDistSq) < (radSum + collTolerance) * (radSum + collTolerance)) { Vector3 segPos = oldSeg.GetPoint(oldt); Vector3 delta = oldSphere.Position - segPos; float dist = (float)System.Math.Sqrt((float)oldDistSq); float depth = radSum - dist; if (dist > JiggleMath.Epsilon) { delta /= dist; } else { // todo - make this not random delta = Vector3Extensions.TransformNormal(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360)))); } Vector3 worldPos = segPos + ((oldCapsule.Radius - 0.5f * depth) * delta); unsafe { SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1); } } }
/// <summary> /// Update stuff at the end of physics /// </summary> /// <param name="dt"></param> public void PostPhysics(float dt) { for (int i = 0; i < wheels.Count; i++) { wheels[i].Update(dt); } // control inputs float deltaAccelerate = dt * 4.0f; float deltaSteering = dt * steerRate; // update the actual values float dAccelerate = destAccelerate - accelerate; dAccelerate = OpenTKHelper.Clamp(dAccelerate, -deltaAccelerate, deltaAccelerate); accelerate += dAccelerate; float dSteering = destSteering - steering; dSteering = OpenTKHelper.Clamp(dSteering, -deltaSteering, deltaSteering); steering += dSteering; // apply these inputs float maxTorque = driveTorque; if (fWDrive && bWDrive) { maxTorque *= 0.5f; } if (fWDrive) { wheels[(int)WheelId.WheelFL].AddTorque(maxTorque * accelerate); wheels[(int)WheelId.WheelFR].AddTorque(maxTorque * accelerate); } if (bWDrive) { wheels[(int)WheelId.WheelBL].AddTorque(maxTorque * accelerate); wheels[(int)WheelId.WheelBR].AddTorque(maxTorque * accelerate); } wheels[(int)WheelId.WheelBL].Lock = (hBrake > 0.5f); wheels[(int)WheelId.WheelBR].Lock = (hBrake > 0.5f); // steering angle applies to the inner wheel. The outer one needs to match it int inner, outer; if (steering > 0.0f) { inner = (int)WheelId.WheelFL; outer = (int)WheelId.WheelFR; } else { inner = (int)WheelId.WheelFR; outer = (int)WheelId.WheelFL; } float alpha = System.Math.Abs(maxSteerAngle * steering); float angleSgn = steering > 0.0f ? 1.0f : -1.0f; wheels[inner].SteerAngle = (angleSgn * alpha); float beta; if (alpha == 0.0f) { beta = alpha; } else { float dx = (wheels[(int)WheelId.WheelFR].Pos.X - wheels[(int)WheelId.WheelBR].Pos.X); float dy = (wheels[(int)WheelId.WheelFL].Pos.Z - wheels[(int)WheelId.WheelFR].Pos.Z); //beta = ATan2Deg(dy, dx + (dy / TanDeg(alpha))); beta = (float)System.Math.Atan2(OpenTKHelper.ToRadians(dy), OpenTKHelper.ToRadians(dx + (dy / (float)System.Math.Tan(OpenTKHelper.ToRadians(alpha))))); beta = OpenTKHelper.ToDegrees(beta); } wheels[outer].SteerAngle = (angleSgn * beta); }
/// <summary> /// Assumes dir is normalised. Angle is in degrees. /// </summary> /// <param name="ang"></param> /// <param name="dir"></param> /// <returns>Matrix4</returns> public static Matrix4 RotationMatrix(float ang, Vector3 dir) { return(Matrix4.CreateFromAxisAngle(dir, OpenTKHelper.ToRadians(ang))); }
/// <summary> // TODO teting testing testing ... /// Adds the forces die to this wheel to the parent. Return value indicates if it's /// on the ground. /// </summary> /// <param name="dt"></param> public bool AddForcesToCar(float dt) { Vector3 force = Vector3.Zero; lastDisplacement = displacement; displacement = 0.0f; Body carBody = car.Chassis.Body; Vector3 worldPos = carBody.Position + Vector3Extensions.TransformNormal(pos, carBody.Orientation); // *mPos; Vector3 worldAxis = Vector3Extensions.TransformNormal(axisUp, carBody.Orientation); // *mAxisUp; //Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.Orientation.GetCol(0); // OpenGl has differnet row/column order for matrixes than XNA has .. Vector3 wheelFwd = Vector3Extensions.TransformNormal(carBody.Orientation.Right(), JiggleMath.RotationMatrix(steerAngle, worldAxis)); //Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.GetOrientation().GetCol(0); Vector3 wheelUp = worldAxis; Vector3 wheelLeft = Vector3.Cross(wheelUp, wheelFwd); wheelLeft.Normalize(); wheelUp = Vector3.Cross(wheelFwd, wheelLeft); // start of ray float rayLen = 2.0f * radius + travel; Vector3 wheelRayEnd = worldPos - radius * worldAxis; Segment wheelRay = new Segment(wheelRayEnd + rayLen * worldAxis, -rayLen * worldAxis); //Assert(PhysicsSystem.CurrentPhysicsSystem); CollisionSystem collSystem = PhysicsSystem.CurrentPhysicsSystem.CollisionSystem; //Assert(collSystem); int numRaysUse = System.Math.Min(numRays, maxNumRays); // adjust the start position of the ray - divide the wheel into numRays+2 // rays, but don't use the first/last. float deltaFwd = (2.0f * radius) / (numRaysUse + 1); float deltaFwdStart = deltaFwd; lastOnFloor = false; int bestIRay = 0; int iRay; for (iRay = 0; iRay < numRaysUse; ++iRay) { fracs[iRay] = float.MaxValue; //SCALAR_HUGE; // work out the offset relative to the middle ray float distFwd = (deltaFwdStart + iRay * deltaFwd) - radius; //float zOffset = mRadius * (1.0f - CosDeg(90.0f * (distFwd / mRadius))); float zOffset = radius * (1.0f - (float)System.Math.Cos(OpenTKHelper.ToRadians(90.0f * (distFwd / radius)))); segments[iRay] = wheelRay; segments[iRay].Origin += distFwd * wheelFwd + zOffset * wheelUp; if (collSystem.SegmentIntersect(out fracs[iRay], out otherSkins[iRay], out groundPositions[iRay], out groundNormals[iRay], segments[iRay], pred)) { lastOnFloor = true; if (fracs[iRay] < fracs[bestIRay]) { bestIRay = iRay; } } } if (!lastOnFloor) { return(false); } //Assert(bestIRay < numRays); // use the best one Vector3 groundPos = groundPositions[bestIRay]; float frac = fracs[bestIRay]; CollisionSkin otherSkin = otherSkins[bestIRay]; // const Vector3 groundNormal = (worldPos - segments[bestIRay].GetEnd()).NormaliseSafe(); // const Vector3 groundNormal = groundNormals[bestIRay]; Vector3 groundNormal = worldAxis; if (numRaysUse > 1) { for (iRay = 0; iRay < numRaysUse; ++iRay) { if (fracs[iRay] <= 1.0f) { groundNormal += (1.0f - fracs[iRay]) * (worldPos - segments[iRay].GetEnd()); } } JiggleMath.NormalizeSafe(ref groundNormal); } else { groundNormal = groundNormals[bestIRay]; } //Assert(otherSkin); Body worldBody = otherSkin.Owner; displacement = rayLen * (1.0f - frac); displacement = OpenTKHelper.Clamp(displacement, 0, travel); float displacementForceMag = displacement * spring; // reduce force when suspension is par to ground displacementForceMag *= Vector3.Dot(groundNormals[bestIRay], worldAxis); // apply damping float dampingForceMag = upSpeed * damping; float totalForceMag = displacementForceMag + dampingForceMag; if (totalForceMag < 0.0f) { totalForceMag = 0.0f; } Vector3 extraForce = totalForceMag * worldAxis; force += extraForce; // side-slip friction and drive force. Work out wheel- and floor-relative coordinate frame Vector3 groundUp = groundNormal; Vector3 groundLeft = Vector3.Cross(groundNormal, wheelFwd); JiggleMath.NormalizeSafe(ref groundLeft); Vector3 groundFwd = Vector3.Cross(groundLeft, groundUp); Vector3 wheelPointVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3Extensions.TransformNormal(pos, carBody.Orientation));// * mPos); Vector3 rimVel = angVel * Vector3.Cross(wheelLeft, groundPos - worldPos); wheelPointVel += rimVel; // if sitting on another body then adjust for its velocity. if (worldBody != null) { Vector3 worldVel = worldBody.Velocity + Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position); wheelPointVel -= worldVel; } // sideways forces float noslipVel = 0.2f; float slipVel = 0.4f; float slipFactor = 0.7f; float smallVel = 3; float friction = sideFriction; float sideVel = Vector3.Dot(wheelPointVel, groundLeft); if ((sideVel > slipVel) || (sideVel < -slipVel)) { friction *= slipFactor; } else if ((sideVel > noslipVel) || (sideVel < -noslipVel)) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel); } if (sideVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(sideVel) < smallVel) { friction *= System.Math.Abs(sideVel) / smallVel; } float sideForce = -friction * totalForceMag; extraForce = sideForce * groundLeft; force += extraForce; // fwd/back forces friction = fwdFriction; float fwdVel = Vector3.Dot(wheelPointVel, groundFwd); if ((fwdVel > slipVel) || (fwdVel < -slipVel)) { friction *= slipFactor; } else if ((fwdVel > noslipVel) || (fwdVel < -noslipVel)) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel); } if (fwdVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(fwdVel) < smallVel) { friction *= System.Math.Abs(fwdVel) / smallVel; } float fwdForce = -friction * totalForceMag; extraForce = fwdForce * groundFwd; force += extraForce; //if (!force.IsSensible()) //{ // TRACE_FILE_IF(ONCE_1) // TRACE("Bad force in car wheel\n"); // return true; //} // fwd force also spins the wheel Vector3 wheelCentreVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3Extensions.TransformNormal(pos, carBody.Orientation));// * mPos); angVelForGrip = Vector3.Dot(wheelCentreVel, groundFwd) / radius; torque += -fwdForce * radius; // add force to car carBody.AddWorldForce(force, groundPos); //if (float.IsNaN(force.X)) // while(true){} //System.Diagnostics.Debug.WriteLine(force.ToString()); // add force to the world if (worldBody != null && !worldBody.Immovable) { // todo get the position in the right place... // also limit the velocity that this force can produce by looking at the // mass/inertia of the other object float maxOtherBodyAcc = 500.0f; float maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass; if (force.LengthSquared > (maxOtherBodyForce * maxOtherBodyForce)) { force *= maxOtherBodyForce / force.Length; } worldBody.AddWorldForce(-force, groundPos); } return(true); }
/// <summary> /// CollDetect /// </summary> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Sphere oldSphere0 = (Sphere)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0); Sphere newSphere0 = (Sphere)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0); Sphere oldSphere1 = (Sphere)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1); Sphere newSphere1 = (Sphere)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1); Vector3 oldDelta = oldSphere0.Position - oldSphere1.Position; Vector3 newDelta = newSphere0.Position - oldSphere1.Position; float oldDistSq = oldDelta.LengthSquared; float newDistSq = newDelta.LengthSquared; float radSum = newSphere0.Radius + newSphere1.Radius; if (System.Math.Min(oldDistSq, newDistSq) < ((radSum + collTolerance) * (radSum + collTolerance))) { float oldDist = (float)System.Math.Sqrt((float)oldDistSq); float depth = radSum - oldDist; if (oldDist > JiggleMath.Epsilon) { oldDelta /= oldDist; } else { // TODO - make this not random...! oldDelta = Vector3Extensions.TransformNormal(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360)))); } Vector3 worldPos = oldSphere1.Position + (oldSphere1.Radius - 0.5f * depth) * oldDelta; unsafe { SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref oldDelta, &collInfo, 1); } } }