public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
TriangleMeshShape triangle = this.Shape1 as TriangleMeshShape;
SphereShape sphere = this.Shape2 as SphereShape;
// Get the center of sphere in world coordinates -> center1
triangle.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of triangle in world coordinates -> center2
sphere.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector[] vertices = triangle.Vertices;
TSVector.Transform(ref vertices[0], ref orientation1, out vertices[0]);
TSVector.Add(ref position1, ref vertices[0], out vertices[0]);
TSVector.Transform(ref vertices[1], ref orientation1, out vertices[1]);
TSVector.Add(ref position1, ref vertices[1], out vertices[1]);
TSVector.Transform(ref vertices[2], ref orientation1, out vertices[2]);
TSVector.Add(ref position1, ref vertices[2], out vertices[2]);
return(Collide(center2, sphere.radius, ref vertices, ref point, ref point1, ref point2, ref normal, ref penetration));
}
protected override Multishape CreateWorkingClone()
{
TriangleMeshShape clone = new TriangleMeshShape(this.octree);
clone.sphericalExpansion = this.sphericalExpansion;
return(clone);
}
//https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
private bool RayIntersectsTriangle(ISupportMappable support, ref TSMatrix orientation, ref TSMatrix invOrientation,
ref TSVector position, ref TSVector origin, ref TSVector direction, out FP fraction, out TSVector normal)
{
FP EPSILON = FP.EN8;
fraction = FP.Zero;
normal = TSVector.zero;
TriangleMeshShape inTriangle = support as TriangleMeshShape;
TSVector[] vertices = inTriangle.Vertices;
TSVector vertex0 = vertices[0];
TSVector vertex1 = vertices[1];
TSVector vertex2 = vertices[2];
TSVector edge1, edge2, h, s, q;
FP a, f, u, v;
edge1 = inTriangle.edge1;
edge2 = inTriangle.edge2;
h = TSVector.Cross(direction, edge2);
a = TSVector.Dot(edge1, h);
if (a > -EPSILON && a < EPSILON)
{
return(false);
}
f = 1 / a;
s = origin - vertex0;
u = f * (TSVector.Dot(s, h));
if (u < FP.Zero || u > FP.One)
{
return(false);
}
q = TSVector.Cross(s, edge1);
v = f * TSVector.Dot(direction, q);
if (v < FP.Zero || u + v > FP.One)
{
return(false);
}
// At this stage we can compute t to find out where the intersection point is on the line.
fraction = f * TSVector.Dot(edge2, q);
if (fraction > EPSILON) // ray intersection
{
return(true);
}
else // This means that there is a line intersection but not a ray intersection.
{
return(false);
}
}
