/// <summary> /// Indicates if a segment intersects a triangle /// </summary> /// <param name="seg"></param> /// <param name="triangle"></param> /// <returns></returns> public static bool SegmentTriangleOverlap(Segment seg, Triangle triangle) { /// the parameters - if hit then they get copied into the args float u, v, t; Vector3 e1 = triangle.Edge0; Vector3 e2 = triangle.Edge1; Vector3 p = Vector3.Cross(seg.Delta, e2); float a = Vector3.Dot(e1, p); if (a > -JiggleMath.Epsilon && a < JiggleMath.Epsilon) return false; float f = 1.0f / a; Vector3 s = seg.Origin - triangle.Origin; u = f * Vector3.Dot(s, p); if (u < 0.0f || u > 1.0f) return false; Vector3 q = Vector3.Cross(s, e1); v = f * Vector3.Dot(seg.Delta, q); if (v < 0.0f || (u + v) > 1.0f) return false; t = f * Vector3.Dot(e2, q); if (t < 0.0f || t > 1.0f) return false; return true; }
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 #region Get Cylinders Cylinder oldCylinder0 = (Cylinder)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0); Cylinder newCylinder0 = (Cylinder)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0); Cylinder oldCylinder1 = (Cylinder)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1); Cylinder newCylinder1 = (Cylinder)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1); #endregion Segment oldSeg0 = new Segment(oldCylinder0.Position, oldCylinder0.Length * MatrixHelper.GetBackward(oldCylinder0.Orientation)); Segment newSeg0 = new Segment(newCylinder0.Position, newCylinder0.Length * MatrixHelper.GetBackward(newCylinder0.Orientation)); Segment oldSeg1 = new Segment(oldCylinder1.Position, oldCylinder1.Length * MatrixHelper.GetBackward(oldCylinder1.Orientation)); Segment newSeg1 = new Segment(newCylinder1.Position, newCylinder1.Length * MatrixHelper.GetBackward(newCylinder1.Orientation)); float radSum = newCylinder0.Radius + newCylinder1.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 = Vector3.TransformCoordinate(Vector3Helper.Backward, Matrix.RotationAxis(Vector3Helper.Up, MathHelper.ToRadians(random.Next(360)))); } Vector3 worldPos = pos1 + (oldCylinder1.Radius - 0.5f * depth) * delta; unsafe { SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1); } } }
/// <summary> /// Indicates if a segment overlaps an AABox /// </summary> /// <param name="seg"></param> /// <param name="AABox"></param> /// <returns></returns> public static bool SegmentAABoxOverlap(Segment seg, AABox AABox) { Vector3 p0 = seg.Origin; Vector3 p1 = seg.GetEnd(); float[] faceOffsets = new float[2]; // The AABox faces are aligned with the world directions. Loop // over the 3 directions and do the two tests. for (int iDir = 0; iDir < 3; iDir++) { int jDir = (iDir + 1) % 3; int kDir = (iDir + 2) % 3; // one plane goes through the origin, one is offset faceOffsets[0] = JiggleUnsafe.Get(AABox.MinPos, iDir); faceOffsets[1] = JiggleUnsafe.Get(AABox.MaxPos, iDir); for (int iFace = 0; iFace < 2; iFace++) { // distance of each point from to the face plane float dist0 = JiggleUnsafe.Get(ref p0, iDir) - faceOffsets[iFace]; float dist1 = JiggleUnsafe.Get(ref p1, iDir) - faceOffsets[iFace]; float frac = -1.0f; if (dist0 * dist1 < -JiggleMath.Epsilon) frac = -dist0 / (dist1 - dist0); else if (System.Math.Abs(dist0) < JiggleMath.Epsilon) frac = 0.0f; else if (System.Math.Abs(dist1) < JiggleMath.Epsilon) frac = 1.0f; if (frac >= 0.0f) { //Assert(frac <= 1.0f); Vector3 pt = seg.GetPoint(frac); // check the point is within the face rectangle if ((JiggleUnsafe.Get(ref pt, jDir) > JiggleUnsafe.Get(AABox.MinPos, jDir) - JiggleMath.Epsilon) && (JiggleUnsafe.Get(ref pt, jDir) < JiggleUnsafe.Get(AABox.MaxPos, jDir) + JiggleMath.Epsilon) && (JiggleUnsafe.Get(ref pt, kDir) > JiggleUnsafe.Get(AABox.MinPos, kDir) - JiggleMath.Epsilon) && (JiggleUnsafe.Get(ref pt, kDir) < JiggleUnsafe.Get(AABox.MaxPos, kDir) + JiggleMath.Epsilon)) { return true; } } } } return false; }
public static bool SegmentCapsuleIntersection(out float tS, Segment seg, Capsule capsule) { float bestFrac = float.MaxValue; tS = 0; // do the main sides float sideFrac = float.MaxValue; if (!SegmentInfiniteCylinderIntersection(out sideFrac, seg, new Segment(capsule.Position, MatrixHelper.GetBackward(capsule.Orientation)), capsule.Radius)) return false; // check this // only keep this if the side intersection point is within the capsule segment ends Vector3 sidePos = seg.GetPoint(sideFrac); if (Vector3.Dot(sidePos - capsule.Position, MatrixHelper.GetBackward(capsule.Orientation)) < 0.0f) sideFrac = float.MaxValue; else if (Vector3.Dot(sidePos - capsule.GetEnd(), MatrixHelper.GetBackward(capsule.Orientation)) > 0.0f) sideFrac = float.MaxValue; // do the two ends float originFrac = float.MaxValue; SegmentSphereIntersection(out originFrac, seg, new Sphere(capsule.Position, capsule.Radius)); float endFrac = float.MaxValue; // Check this! SegmentSphereIntersection(out endFrac, seg, new Sphere(capsule.GetEnd(), capsule.Radius)); bestFrac = MathHelper.Min(sideFrac, originFrac); bestFrac = MathHelper.Min(bestFrac, endFrac); if (bestFrac <= 1.0f) { tS = bestFrac; return true; } return false; }
public static bool SegmentTriangleIntersection(out float tS, out float tT0, out float tT1, Segment seg, Triangle triangle) { /// the parameters - if hit then they get copied into the args float u, v, t; tS = 0; tT0 = 0; tT1 = 0; Vector3 e1 = triangle.Edge0; Vector3 e2 = triangle.Edge1; Vector3 p = Vector3.Cross(seg.Delta, e2); float a = Vector3.Dot(e1, p); if (a > -JiggleMath.Epsilon && a < JiggleMath.Epsilon) return false; float f = 1.0f / a; Vector3 s = seg.Origin - triangle.Origin; u = f * Vector3.Dot(s, p); if (u < 0.0f || u > 1.0f) return false; Vector3 q = Vector3.Cross(s, e1); v = f * Vector3.Dot(seg.Delta, q); if (v < 0.0f || (u + v) > 1.0f) return false; t = f * Vector3.Dot(e2, q); if (t < 0.0f || t > 1.0f) return false; tS = t; tT0 = u; tT1 = v; //if (tS != 0) tS = t; //if (tT0 != 0) tT0 = u; //if (tT1 != 0) tT1 = v; return true; }
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg) { // move segment into octree space seg.Origin = Vector3.TransformCoordinate(seg.Origin, invTransform); seg.Delta = Vector3.TransformNormal(seg.Delta, invTransform); BoundingBox segBox = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSegment(seg, ref segBox); unsafe { #if USE_STACKALLOC int* potentialTriangles = stackalloc int[MaxLocalStackTris]; { #else int[] potTriArray = DetectFunctor.IntStackAlloc(); fixed (int* potentialTriangles = potTriArray) { #endif int numTriangles = GetTrianglesIntersectingtAABox(potentialTriangles, DetectFunctor.MaxLocalStackTris, ref segBox); float tv1, tv2; pos = Vector3.Zero; normal = Vector3.Zero; float bestFrac = float.MaxValue; for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { IndexedTriangle meshTriangle = GetTriangle(potentialTriangles[iTriangle]); float thisFrac; Triangle tri = new Triangle(GetVertex(meshTriangle.GetVertexIndex(0)), GetVertex(meshTriangle.GetVertexIndex(1)), GetVertex(meshTriangle.GetVertexIndex(2))); if (Intersection.SegmentTriangleIntersection(out thisFrac, out tv1, out tv2, seg, tri)) { if (thisFrac < bestFrac) { bestFrac = thisFrac; // re-project pos = Vector3.TransformCoordinate(seg.GetPoint(thisFrac), transformMatrix); normal = Vector3.TransformNormal(meshTriangle.Plane.Normal, transformMatrix); } } } frac = bestFrac; if (bestFrac < float.MaxValue) { DetectFunctor.FreeStackAlloc(potTriArray); return true; } else { DetectFunctor.FreeStackAlloc(potTriArray); return false; } #if USE_STACKALLOC } #else } #endif } }
public static bool SegmentPlaneIntersection(out float tS, Segment seg, Plane plane) { float denom = Vector3.Dot(plane.Normal, seg.Delta); if (System.Math.Abs(denom) > JiggleMath.Epsilon) { float t = -(Vector3.Dot(plane.Normal, seg.Origin) + plane.D) / denom; if (t < 0.0f || t > 1.0f) { tS = 0.0f; return false; } tS = t; return true; } else { // parallel - return false even if it's in the plane tS = 0.0f; return false; } }
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 = MathHelper.Max(lambda1, 0.0f); return true; }
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg) { bool result; if (result = Intersection.SegmentPlaneIntersection(out frac, seg, this)) { pos = seg.GetPoint(frac); normal = this.Normal; } else { pos = Vector3.Zero; normal = Vector3.Zero; } return result; }
public static bool SegmentInfiniteCylinderIntersection(out float tS, Segment seg, Segment cylinderAxis, float radius) { Vector3 Ks = seg.Delta; float kss = Vector3.Dot(Ks, Ks); float radiusSq = radius * radius; Vector3 Ke = cylinderAxis.Delta; Vector3 Kg = cylinderAxis.Origin - seg.Origin; tS = 0.0f; float kee = Vector3.Dot(Ke, Ke); if (System.Math.Abs(kee) < JiggleMath.Epsilon) return false; float kes = Vector3.Dot(Ke, Ks); float kgs = Vector3.Dot(Kg, Ks); float keg = Vector3.Dot(Ke, Kg); float kgg = Vector3.Dot(Kg, Kg); // check if start is inside float distSq = (Kg - (keg * Ke) / kee).LengthSquared(); if (distSq < radiusSq) return true; // a * t^2 + b * t + c = 0 float a = kee * kss - kes * kes; if (System.Math.Abs(a) < JiggleMath.Epsilon) return false; float b = 2.0f * (keg * kes - kee * kgs); float c = kee * (kgg - radiusSq) - keg * keg; float blah = b * b - 4.0f * a * c; if (blah < 0.0f) return false; // solve for t - take minimum float t = (-b - (float)System.Math.Sqrt((float)blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) return false; tS = t; return true; }
/// <summary> /// Intersect a segment with the world. If non-zero the predicate /// allows certain skins to be excluded /// </summary> /// <param name="seg"></param> /// <param name="collisionPredicate"></param> /// <returns></returns> public abstract bool SegmentIntersect(out float fracOut, out CollisionSkin skinOut, out Vector3 posOut, out Vector3 normalOut, Segment seg, CollisionSkinPredicate1 collisionPredicate);
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg) { bool result = Intersection.SegmentCapsuleIntersection(out frac, seg, this); if (result) { Vector3 orientationBackward = MatrixHelper.GetBackward(transform.Orientation); pos = seg.GetPoint(frac); normal = pos - transform.Position; normal -= Vector3.Dot(normal, orientationBackward) * orientationBackward; JiggleMath.NormalizeSafe(ref normal); } else { pos = normal = Vector3.Zero; } return result; }
/// <summary> /// GetBoxTriangleIntersectionPoints /// Pushes intersection points onto the back of pts. Returns the /// number of points found. /// Points that are close together (compared to /// combinationDistance) get combined /// </summary> /// <param name="pts"></param> /// <param name="box"></param> /// <param name="triangle"></param> /// <param name="combinationDistance"></param> /// <returns></returns> private static int GetBoxTriangleIntersectionPoints(List<Vector3> pts, Box box, Triangle triangle, float combinationDistance) { // first intersect each edge of the box with the triangle Box.Edge[] edges; box.GetEdges(out edges); Vector3[] boxPts; box.GetCornerPoints(out boxPts); float tS; float tv1, tv2; int iEdge; for (iEdge = 0; iEdge < 12; ++iEdge) { Box.Edge edge = edges[iEdge]; Segment seg = new Segment(boxPts[(int)edge.Ind0], boxPts[(int)edge.Ind1] - boxPts[(int)edge.Ind0]); if (Intersection.SegmentTriangleIntersection(out tS, out tv1, out tv2, seg, triangle)) { AddPoint(pts, seg.GetPoint(tS), combinationDistance * combinationDistance); } } Vector3 pos, n; // now each edge of the triangle with the box for (iEdge = 0; iEdge < 3; ++iEdge) { Vector3 pt0 = triangle.GetPoint(iEdge); Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3); Segment s1 = new Segment(pt0, pt1 - pt0); Segment s2 = new Segment(pt1, pt0 - pt1); if (box.SegmentIntersect(out tS, out pos, out n, s1)) AddPoint(pts, pos, combinationDistance * combinationDistance); if (box.SegmentIntersect(out tS, out pos, out n, s2)) AddPoint(pts, pos, combinationDistance * combinationDistance); } return pts.Count; }
/// <summary> /// /// </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 * MatrixHelper.GetBackward(oldCapsule.Orientation)); Segment newSeg = new Segment(newCapsule.Position, newCapsule.Length * MatrixHelper.GetBackward(newCapsule.Orientation)); 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 (MathHelper.Min(oldDistSq, newDistSq) < ((radius + collTolerance) * (radius + collTolerance))) { Matrix oldBoxOrientation = oldBox.Orientation; Vector3 segPos = oldSeg.GetPoint(oldSegT); Vector3 boxPos = oldBox.GetCentre() + oldBoxT0 * MatrixHelper.GetRight(ref oldBoxOrientation) + oldBoxT1 * MatrixHelper.GetUp(ref oldBoxOrientation) + oldBoxT2 * MatrixHelper.GetBackward(ref oldBoxOrientation); 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.TransformCoordinate(Vector3Helper.Backward, Matrix.RotationAxis(Vector3Helper.Up, MathHelper.ToRadians(random.Next(360)))); } unsafe { SmallCollPointInfo collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1); } } }
/// <summary> /// Must support intersection with a segment (ray cast) /// </summary> /// <param name="frac"></param> /// <param name="normal"></param> /// <param name="seg"></param> /// <returns></returns> public abstract bool SegmentIntersect(out float frac,out Vector3 pos, out Vector3 normal,Segment seg);
/// <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 + Vector3.TransformCoordinate(pos, carBody.Orientation);// *mPos; Vector3 worldAxis = Vector3.TransformCoordinate(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 = Vector3.TransformCoordinate(MatrixHelper.GetRight(carBody.Orientation), 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( MathHelper.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 = MathHelper.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, Vector3.TransformCoordinate( 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, Vector3.TransformCoordinate(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> /// Every skin must support a ray/segment intersection test - /// operates on the new value of the primitives /// </summary> /// <param name="frac"></param> /// <param name="pos"></param> /// <param name="normal"></param> /// <param name="seg"></param> /// <returns></returns> public bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg) { Vector3 segEnd = seg.GetEnd(); frac = float.MaxValue; float thisSegLenRelToOrig = 1.0f; Segment segCopy = seg; pos = normal = Vector3.Zero; for (int prim = primitivesNewWorld.Count; prim-- != 0; ) { float thisFrac; Vector3 newPosition = pos; if (primitivesNewWorld[prim].SegmentIntersect(out thisFrac, out newPosition, out normal, segCopy)) { pos = newPosition; frac = thisFrac * thisSegLenRelToOrig; segCopy.Delta *= thisFrac; thisSegLenRelToOrig *= frac; } } //System.Diagnostics.Debug.WriteLineIf(frac <= 1.0f, pos); return (frac <= 1.0f); }
public override bool SegmentIntersect(out float fracOut, out Vector3 posOut, out Vector3 normalOut, Segment seg) { fracOut = float.MaxValue; posOut = normalOut = Vector3.Zero; // algo taken from p674 of realting rendering // needs debugging float min = float.MinValue; float max = float.MaxValue; Vector3 p = GetCentre() - seg.Origin; Vector3 h; h.X = sideLengths.X * 0.5f; h.Y = sideLengths.Y * 0.5f; h.Z = sideLengths.Z * 0.5f; int dirMax = 0; int dirMin = 0; int dir = 0; Matrix orientation = transform.Orientation; Vector3[] matrixVec = new Vector3[3]; MatrixHelper.GetRight(ref orientation, out matrixVec[0]); //matrixVec[0] = transform.Orientation.Right; MatrixHelper.GetUp(ref orientation, out matrixVec[1]); //matrixVec[1] = transform.Orientation.Up; MatrixHelper.GetBackward(ref orientation, out matrixVec[2]); //matrixVec[2] = transform.Orientation.Backward; float[] vectorFloat = new float[3]; vectorFloat[0] = h.X; vectorFloat[1] = h.Y; vectorFloat[2] = h.Z; for (dir = 0; dir < 3; dir++) { float e = Vector3.Dot(matrixVec[dir], p); float f = Vector3.Dot(matrixVec[dir], seg.Delta); if (System.Math.Abs(f) > JiggleMath.Epsilon) { float t1 = (e + vectorFloat[dir]) / f; float t2 = (e - vectorFloat[dir]) / f; if (t1 > t2){float tmp = t1;t1 = t2; t2 = tmp;} if (t1 > min) { min = t1; dirMin = dir; } if (t2 < max) { max = t2; dirMax = dir; } if (min > max) return false; if (max < 0.0f) return false; } else if((-e-vectorFloat[dir] > 0.0f) || (-e + vectorFloat[dir] < 0.0f)) { return false; } } if (min > 0.0f) { dir = dirMin; fracOut = min; } else { dir = dirMax; fracOut = max; } fracOut = MathHelper.Clamp(fracOut, 0.0f, 1.0f); posOut = seg.GetPoint(fracOut); if (Vector3.Dot(matrixVec[dir], seg.Delta) > 0.0f) normalOut = -matrixVec[dir]; else normalOut = matrixVec[dir]; return true; }
public override bool SegmentIntersect(out float fracOut,out CollisionSkin skinOut,out Vector3 posOut,out Vector3 normalOut, Segment seg, CollisionSkinPredicate1 collisionPredicate) { int numSkins = skins.Count; BoundingBox segBox = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSegment(seg, ref segBox); //initialise the outputs fracOut = float.MaxValue; skinOut = null; posOut = normalOut = Vector3.Zero; // working vars float frac; Vector3 pos; Vector3 normal; for (int iskin = 0; iskin < numSkins; ++iskin) { CollisionSkin skin = skins[iskin]; if ((collisionPredicate == null) || collisionPredicate.ConsiderSkin(skin)) { // basic bbox test if (BoundingBoxHelper.OverlapTest(ref skin.WorldBoundingBox, ref segBox)) { if (skin.SegmentIntersect(out frac, out pos, out normal, seg)) { if (frac < fracOut) { posOut = pos; normalOut = normal; skinOut = skin; fracOut = frac; } } } } } if (fracOut > 1.0f) return false; fracOut = MathHelper.Clamp(fracOut, 0.0f, 1.0f); return true; }