public List <HitInfo> IntersectTriangle(Triangle triangle)
{
List <HitInfo> res = new List <HitInfo>();
for (int i = 0; i < vertices.Count; i++)
{
LineSegment2D l1 = new LineSegment2D(V2(vertices[i]), V2(vertices[(i + 1) % vertices.Count]));
for (int j = 0; j < 3; j++)
{
LineSegment2D l2 = new LineSegment2D(V2(triangle.Vertex(j)), V2(triangle.Vertex((j + 1) % 3)));
Vector2D point;
if (l1.LineIntersect(l2, out point))
{
double distance = (point - V2(vertices[i])).magnitude;
double segmentLength = l1.Length();
//if (l1.DoLinesIntersect(l2)){
double normalizedDistance = distance / segmentLength;
Vector3D point3D = Vector3D.Lerp(new Vector3D(vertices[i]), new Vector3D(vertices[(i + 1) % vertices.Count]), normalizedDistance);
res.Add(new HitInfo(i, j, point3D.ToVector3()));
}
/*Vector2 intersectionPoint;
* if (LineLineIntersection(from,to, tFrom, tTo,out intersectionPoint)){
* float normalizedDistance = Vector3.Dot(to-from, tTo-tFrom);
* Vector3 point3D = Vector3.Lerp (vertices[i], vertices[(i+1)%vertices.Count], normalizedDistance);
* res.Add(new HitInfo(i,j,point3D));
* }*/
}
}
return(res);
}
private static void CalculateVertexVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
a.World(ci.FeatureA.Index, out pa);
tri.Vertex(ci.FeatureB.Index, out pb);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void CalculateFaceVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
tri.Vertex(ci.FeatureB.Index, out pb);
a.World(a.Face(ci.FeatureA.Index)[0], out pa);
var plane = new Plane(pa, ci.Normal);
plane.ClosestPointTo(ref pb, out pa);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
int[] faceA = a.Face(ci.FeatureA.Index);
Vector3 pa, pb, n;
a.World(faceA[0], out pa);
a.FaceNormal(ci.FeatureA.Index, out n);
Plane planeA = new Plane(pa, n);
Plane planeB = new Plane(tri.V1, tri.Normal);
// vertices of A contained in face of B
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i], out pa);
planeB.ClosestPointTo(ref pa, out pb);
if (tri.Contains(ref pb))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
for (int i = 1; i <= 3; i++)
{
tri.Vertex(i, out pb);
planeA.ClosestPointTo(ref pb, out pa);
if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
// intersection of edges from both faces
Segment ea, eb;
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
a.World(faceA[i], out ea.P2);
for (int j = 1; j <= 3; j++)
{
tri.Edge(j, out eb);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
}
private static void CalculateEdgeVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea;
float sa;
int[] edgeA = a.Edge(ci.FeatureA.Index);
a.World(edgeA[0], out ea.P1);
a.World(edgeA[1], out ea.P2);
tri.Vertex(ci.FeatureB.Index, out pb);
ea.ClosestPointTo(ref pb, out sa, out pa);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
int[] faceA = a.Face(ci.FeatureA.Index);
Vector3 pa, pb, n;
a.World(faceA[0], out pa);
a.FaceNormal(ci.FeatureA.Index, out n);
Plane planeA = new Plane(pa, n);
Plane planeB = new Plane(tri.V1, tri.Normal);
// vertices of A contained in face of B
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i], out pa);
planeB.ClosestPointTo(ref pa, out pb);
if (tri.Contains(ref pb))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
for (int i = 1; i <= 3; i++)
{
tri.Vertex(i, out pb);
planeA.ClosestPointTo(ref pb, out pa);
if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
// intersection of edges from both faces
Segment ea, eb;
for (int i = 0; i < faceA.Length; i++)
{
a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
a.World(faceA[i], out ea.P2);
for (int j = 1; j <= 3; j++)
{
tri.Edge(j, out eb);
float sa, sb;
Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
{
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
}
}
}
private static void CalculateFaceVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
tri.Vertex(ci.FeatureB.Index, out pb);
a.World(a.Face(ci.FeatureA.Index)[0], out pa);
var plane = new Plane(pa, ci.Normal);
plane.ClosestPointTo(ref pb, out pa);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void CalculateEdgeVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
Segment ea;
float sa;
int[] edgeA = a.Edge(ci.FeatureA.Index);
a.World(edgeA[0], out ea.P1);
a.World(edgeA[1], out ea.P2);
tri.Vertex(ci.FeatureB.Index, out pb);
ea.ClosestPointTo(ref pb, out sa, out pa);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
private static void CalculateVertexVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
{
Vector3 pa, pb;
a.World(ci.FeatureA.Index, out pa);
tri.Vertex(ci.FeatureB.Index, out pb);
cf.WritePoint(ref pa, ref pb, ref ci.Normal);
}
public List<HitInfo> IntersectTriangle(Triangle triangle)
{
List<HitInfo> res = new List<HitInfo>();
for (int i=0;i<vertices.Count;i++){
LineSegment2D l1 = new LineSegment2D(V2(vertices[i]), V2(vertices[(i+1)%vertices.Count]));
for (int j=0;j<3;j++){
LineSegment2D l2 = new LineSegment2D(V2(triangle.Vertex(j)), V2(triangle.Vertex((j+1)%3)));
Vector2D point;
if (l1.LineIntersect(l2, out point)){
double distance = (point - V2(vertices[i])).magnitude;
double segmentLength = l1.Length();
//if (l1.DoLinesIntersect(l2)){
double normalizedDistance = distance/segmentLength;
Vector3D point3D = Vector3D.Lerp (new Vector3D (vertices[i]), new Vector3D (vertices[(i+1)%vertices.Count]), normalizedDistance);
res.Add(new HitInfo(i,j,point3D.ToVector3()));
}
/*Vector2 intersectionPoint;
if (LineLineIntersection(from,to, tFrom, tTo,out intersectionPoint)){
float normalizedDistance = Vector3.Dot(to-from, tTo-tFrom);
Vector3 point3D = Vector3.Lerp (vertices[i], vertices[(i+1)%vertices.Count], normalizedDistance);
res.Add(new HitInfo(i,j,point3D));
}*/
}
}
return res;
}
