/// <summary> /// Pushes intersection points (in world space) onto the back of pts. /// Intersection is between an AABox faces and an orientated box's /// edges. orient and pos are used to transform the points from the /// AABox frame back into the original frame. /// </summary> private static int GetAABox2BoxEdgesIntersectionPoints(List <ContactPoint> pts, ref Vector3 sides, MyBox box, ref Matrix origBoxOrient, ref Vector3 origBoxPos, float combinationDistanceSq) { int num = 0; Vector3[] boxPts; box.GetCornerPoints(out boxPts); MyBox.Edge[] edges; box.GetEdges(out edges); for (int iedge = 0; iedge < 12; ++iedge) { Vector3 edgePt0 = boxPts[(int)edges[iedge].Ind0]; Vector3 edgePt1 = boxPts[(int)edges[iedge].Ind1]; num += GetAABox2EdgeIntersectionPoints(pts, ref sides, box, ref edgePt0, ref edgePt1, ref origBoxOrient, ref origBoxPos, combinationDistanceSq); // Don't think we can get more than 8... and anyway if we get too many // then the penetration must be so bad who cares about the details? if (num >= 8) { return(num); } } return(num); }
// 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 private int GetBoxTriangleIntersectionPoints(List <MyCP> pts, MyBox box, MyColDetVoxelTriangle triangle, float combinationDistance) { // first intersect each edge of the box with the triangleVertexes MyBox.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) { MyBox.Edge edge = edges[iEdge]; MySegment seg = new MySegment(boxPts[(int)edge.Ind0], boxPts[(int)edge.Ind1] - boxPts[(int)edge.Ind0]); if (this.SegmentTriangleIntersection(out tS, out tv1, out tv2, seg, triangle)) { float depthA = Vector3.Dot(boxPts[(int)edge.Ind0] - seg.GetPoint(tS), triangle.Normal); float depthB = Vector3.Dot(boxPts[(int)edge.Ind1] - seg.GetPoint(tS), triangle.Normal); AddPoint(pts, seg.GetPoint(tS), triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } } Vector3 pos, n; // now each edge of the triangleVertexes with the box for (iEdge = 0; iEdge < 3; ++iEdge) { Vector3 pt0 = triangle.GetPoint(iEdge); Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3); MySegment s1 = new MySegment(pt0, pt1 - pt0); MySegment s2 = new MySegment(pt1, pt0 - pt1); if (box.SegmentIntersect(out tS, out pos, out n, s1)) { float depthA = Vector3.Dot(pt0 - pos, triangle.Normal); float depthB = Vector3.Dot(pt1 - pos, triangle.Normal); AddPoint(pts, pos, triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } if (box.SegmentIntersect(out tS, out pos, out n, s2)) { float depthA = Vector3.Dot(pt0 - pos, triangle.Normal); float depthB = Vector3.Dot(pt1 - pos, triangle.Normal); AddPoint(pts, pos, triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } } return(pts.Count); }
/// <summary> /// Pushes intersection points (in world space) onto the back of pts. /// Intersection is between an AABox faces and an orientated box's /// edges. orient and pos are used to transform the points from the /// AABox frame back into the original frame. /// </summary> private static int GetAABox2BoxEdgesIntersectionPoints(List<ContactPoint> pts, ref Vector3 sides, MyBox box, ref Matrix origBoxOrient, ref Vector3 origBoxPos, float combinationDistanceSq) { int num = 0; Vector3[] boxPts; box.GetCornerPoints(out boxPts); MyBox.Edge[] edges; box.GetEdges(out edges); for (int iedge = 0; iedge < 12; ++iedge) { Vector3 edgePt0 = boxPts[(int)edges[iedge].Ind0]; Vector3 edgePt1 = boxPts[(int)edges[iedge].Ind1]; num += GetAABox2EdgeIntersectionPoints(pts, ref sides, box, ref edgePt0, ref edgePt1, ref origBoxOrient, ref origBoxPos, combinationDistanceSq); // Don't think we can get more than 8... and anyway if we get too many // then the penetration must be so bad who cares about the details? if (num >= 8) return num; } return num; }
// 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 private int GetBoxTriangleIntersectionPoints(List<MyCP> pts, MyBox box, MyColDetVoxelTriangle triangle, float combinationDistance) { // first intersect each edge of the box with the triangleVertexes MyBox.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) { MyBox.Edge edge = edges[iEdge]; MySegment seg = new MySegment(boxPts[(int)edge.Ind0], boxPts[(int)edge.Ind1] - boxPts[(int)edge.Ind0]); if (this.SegmentTriangleIntersection(out tS, out tv1, out tv2, seg, triangle)) { float depthA = Vector3.Dot(boxPts[(int)edge.Ind0] - seg.GetPoint(tS), triangle.Normal); float depthB = Vector3.Dot(boxPts[(int)edge.Ind1] - seg.GetPoint(tS), triangle.Normal); AddPoint(pts, seg.GetPoint(tS),triangle.Normal, depthA < depthB ? depthA : depthB , combinationDistance * combinationDistance); } } Vector3 pos, n; // now each edge of the triangleVertexes with the box for (iEdge = 0; iEdge < 3; ++iEdge) { Vector3 pt0 = triangle.GetPoint(iEdge); Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3); MySegment s1 = new MySegment(pt0, pt1 - pt0); MySegment s2 = new MySegment(pt1, pt0 - pt1); if (box.SegmentIntersect(out tS, out pos, out n, s1)) { float depthA = Vector3.Dot(pt0 - pos, triangle.Normal); float depthB = Vector3.Dot(pt1 - pos, triangle.Normal); AddPoint(pts, pos, triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } if (box.SegmentIntersect(out tS, out pos, out n, s2)) { float depthA = Vector3.Dot(pt0 - pos, triangle.Normal); float depthB = Vector3.Dot(pt1 - pos, triangle.Normal); AddPoint(pts, pos, triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } } return pts.Count; }