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