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