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