Implement Polyhedron header

include/J3ML/Geometry/Line.h

@@ -0,0 +1,10 @@
+#pragma once
+
+
+namespace J3ML::Geometry
+{
+    class Line
+    {
+
+    };
+}

include/J3ML/Geometry/Plane.h

@@ -1,6 +1,7 @@
 #pragma once
 #include <J3ML/LinearAlgebra/Vector3.h>
 #include "Shape.h"
+#include "Ray.h"
 
 
 namespace J3ML::Geometry
@@ -25,5 +26,7 @@ namespace J3ML::Geometry
         {
 
         }
+
+        bool Intersects(J3ML::Geometry::Ray ray, float *pDouble);
     };
 }

include/J3ML/Geometry/Polyhedron.h

@@ -66,8 +66,27 @@ namespace J3ML::Geometry
         bool Intersects(const Sphere&) const;
         bool Intersects(const Capsule&) const;
 
+        float FaceContainmentDistance2D(int i, Vector3 vector3) const;
 
+        bool IsClosed() const;
+
+        Plane FacePlane(int faceIndex) const;
+
+        std::vector<Polygon> Faces() const;
+
+        int NumEdges() const;
+
+        LineSegment Edge(int edgeIndex) const;
+
+        std::vector<std::pair<int, int>> EdgeIndices() const;
+
+        std::vector<LineSegment> Edges() const;
+
+        Polygon FacePolygon(int faceIndex) const;
+
+        bool IsConvex() const;
     protected:
     private:
+
     };
 }

src/J3ML/Geometry/Polyhedron.cpp

@@ -3,9 +3,20 @@
 #include <J3ML/Geometry/Triangle.h>
 #include <J3ML/Geometry/LineSegment.h>
 #include "J3ML/Geometry/Ray.h"
+#include <J3ML/Geometry/Polygon.h>
+#include <set>
 
 namespace J3ML::Geometry
 {
+
+    int Polyhedron::NumEdges() const
+    {
+        int numEdges = 0;
+        for(size_t i = 0; i < f.size(); ++i)
+            numEdges += (int)f[i].v.size();
+        return numEdges / 2;
+    }
+
     AABB Polyhedron::MinimalEnclosingAABB() const {
         AABB aabb;
         aabb.SetNegativeInfinity();
@@ -18,6 +29,61 @@ namespace J3ML::Geometry
         return v[vertexIndex];
     }
 
+    LineSegment Polyhedron::Edge(int edgeIndex) const
+    {
+        assert(edgeIndex >= 0);
+        std::vector<LineSegment> edges = Edges();
+        assert(edgeIndex < (int)edges.size());
+        return edges[edgeIndex];
+    }
+
+    Polygon Polyhedron::FacePolygon(int faceIndex) const
+    {
+        Polygon p;
+        assert(faceIndex >= 0);
+        assert(faceIndex < (int)f.size());
+
+        p.vertices.reserve(f[faceIndex].v.size());
+        for(size_t i = 0; i < f[faceIndex].v.size(); ++i)
+            p.vertices.push_back(Vertex(f[faceIndex].v[i]));
+        return p;
+    }
+
+
+    std::vector<LineSegment> Polyhedron::Edges() const
+    {
+        std::vector<std::pair<int, int> > edges = EdgeIndices();
+        std::vector<LineSegment> edgeLines;
+        edgeLines.reserve(edges.size());
+        for(size_t i = 0; i < edges.size(); ++i)
+            edgeLines.push_back(LineSegment(Vertex(edges[i].first), Vertex(edges[i].second)));
+
+        return edgeLines;
+    }
+
+    std::vector<std::pair<int, int> > Polyhedron::EdgeIndices() const
+    {
+        std::set<std::pair<int, int> > uniqueEdges;
+        for(int i = 0; i < NumFaces(); ++i)
+        {
+            assert(f[i].v.size() >= 3);
+            if (f[i].v.size() < 3)
+                continue; // Degenerate face with less than three vertices, skip!
+            int x = f[i].v.back();
+            for(size_t j = 0; j < f[i].v.size(); ++j)
+            {
+                int y = f[i].v[j];
+                uniqueEdges.insert(std::make_pair(std::min(x, y), std::max(x, y)));
+                x = y;
+            }
+        }
+
+        std::vector<std::pair<int, int> >edges;
+        edges.insert(edges.end(), uniqueEdges.begin(), uniqueEdges.end());
+        return edges;
+    }
+
+
     bool Polyhedron::Contains(const Vector3 &point) const {
 
         if (v.size() <= 3) {
@@ -70,10 +136,283 @@ namespace J3ML::Geometry
                 Plane p((Vector3)v[f[i].v[0]] - point, (Vector3)v[f[i].v[1]] - point, (Vector3)v[f[i].v[2]] - point);
 
                 float d;
+                // Find the intersection of the plane and the ray.
+                if (p.Intersects(r, &d))
+                {
+                    float containmentDistance2D = FaceContainmentDistance2D(i, r.GetPoint(d) + point);
+                    if (containmentDistance2D >= 0.f)
+                        ++numIntersections;
+                    faceContainmentDistance = std::min(faceContainmentDistance, std::abs(containmentDistance2D));
+                }
+            }
+            if (faceContainmentDistance > 1e-2f) // The nearest edge was far enough, we conclude the result is believable.
+                return (numIntersections % 2) == 1;
+            else if (faceContainmentDistance >= bestFaceContainmentDistance)
+            {
+                // The ray passed too close to a face edge. Remember this result, but proceed to another test iteration to see if we can
+                // find a more plausible test ray.
+                bestNumIntersections = numIntersections;
+                bestFaceContainmentDistance = faceContainmentDistance;
+            }
+        }
+        // We tested rays through each face of the polyhedron, but all rays passed too close to edges of the polyhedron faces. Return
+        // the result from the test that was farthest to any of the face edges.
+        return (bestNumIntersections % 2) == 1;
+    }
+
+    float Polyhedron::FaceContainmentDistance2D(int i, Vector3 vector3) const {
+        return 0;
+    }
+
+    bool Polyhedron::Contains(const LineSegment &lineSegment) const
+    {
+        return Contains(lineSegment.A) && Contains(lineSegment.B);
+    }
+
+    bool Polyhedron::Contains(const Triangle &triangle) const
+    {
+        return Contains(triangle.V0) && Contains(triangle.V1) && Contains(triangle.V2);
+    }
+
+    bool Polyhedron::Contains(const Polygon &polygon) const
+    {
+        for(int i = 0; i < polygon.NumVertices(); ++i)
+            if (!Contains(polygon.Vertex(i)))
+                return false;
+        return true;
+    }
+
+    bool Polyhedron::Contains(const AABB &aabb) const
+    {
+        for(int i = 0; i < 8; ++i)
+            if (!Contains(aabb.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::Contains(const OBB &obb) const
+    {
+        for(int i = 0; i < 8; ++i)
+            if (!Contains(obb.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::Contains(const Frustum &frustum) const
+    {
+        for(int i = 0; i < 8; ++i)
+            if (!Contains(frustum.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::Contains(const Polyhedron &polyhedron) const
+    {
+        assert(polyhedron.IsClosed());
+        for(int i = 0; i < polyhedron.NumVertices(); ++i)
+            if (!Contains(polyhedron.Vertex(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::IsClosed() const {
 
     }
+
+    Plane Polyhedron::FacePlane(int faceIndex) const
+    {
+        const Face &face = f[faceIndex];
+        if (face.v.size() >= 3)
+            return Plane(v[face.v[0]], v[face.v[1]], v[face.v[2]]);
+        else if (face.v.size() == 2)
+            return Plane(Line(v[face.v[0]], v[face.v[1]]), ((Vector3)v[face.v[0]]-(Vector3)v[face.v[1]]).Perpendicular());
+        else if (face.v.size() == 1)
+            return Plane(v[face.v[0]], DIR_VEC(0,1,0));
+        else
+            return Plane();
+    }
+
+    std::vector<Polygon> Polyhedron::Faces() const
+    {
+        std::vector<Polygon> faces;
+        faces.reserve(NumFaces());
+        for(int i = 0; i < NumFaces(); ++i)
+            faces.push_back(FacePolygon(i));
+        return faces;
+    }
+
+    Vector4 PolyFaceNormal(const Polyhedron &poly, int faceIndex)
+    {
+        const Polyhedron::Face &face = poly.f[faceIndex];
+        if (face.v.size() == 3)
+        {
+            Vector4 a = Vector4(poly.v[face.v[0]], 1.f);
+            Vector4 b = Vector4(poly.v[face.v[1]], 1.f);
+            Vector4 c = Vector4(poly.v[face.v[2]], 1.f);
+            Vector4 normal = (b-a).Cross(c-a);
+            normal.Normalize();
+            return normal;
+//		return ((vec)v[face.v[1]]-(vec)v[face.v[0]]).Cross((vec)v[face.v[2]]-(vec)v[face.v[0]]).Normalized();
+        }
+        else if (face.v.size() > 3)
+        {
+            // Use Newell's method of computing the face normal for best stability.
+            // See Christer Ericson, Real-Time Collision Detection, pp. 491-495.
+            Vector4 normal(0, 0, 0, 0);
+            int v0 = face.v.back();
+            for(size_t i = 0; i < face.v.size(); ++i)
+            {
+                int v1 = face.v[i];
+                normal.x += (double(poly.v[v0].y) - poly.v[v1].y) * (double(poly.v[v0].z) + poly.v[v1].z); // Project on yz
+                normal.y += (double(poly.v[v0].z) - poly.v[v1].z) * (double(poly.v[v0].x) + poly.v[v1].x); // Project on xz
+                normal.z += (double(poly.v[v0].x) - poly.v[v1].x) * (double(poly.v[v0].y) + poly.v[v1].y); // Project on xy
+                v0 = v1;
+            }
+            normal.Normalize();
+            return normal;
+#if 0
+            cv bestNormal;
+		cs bestLen = -FLOAT_INF;
+		cv a = poly.v[face.v[face.v.size()-2]];
+		cv b = poly.v[face.v.back()];
+		for(size_t i = 0; i < face.v.size()-2; ++i)
+		{
+			cv c = poly.v[face.v[i]];
+			cv normal = (c-b).Cross(a-b);
+			float len = normal.Normalize();
+			if (len > bestLen)
+			{
+				bestLen = len;
+				bestNormal = normal;
+			}
+			a = b;
+			b = c;
+		}
+		assert(bestLen != -FLOAT_INF);
+		return DIR_VEC((float)bestNormal.x, (float)bestNormal.y, (float)bestNormal.z);
+#endif
+
+#if 0
+            // Find the longest edge.
+		cs bestLenSq = -FLOAT_INF;
+		cv bestEdge;
+		int v0 = face.v.back();
+		int bestV0 = 0;
+		int bestV1 = 0;
+		for(size_t i = 0; i < face.v.size(); ++i)
+		{
+			int v1 = face.v[i];
+			cv edge = cv(poly.v[v1]) - cv(poly.v[v0]);
+			cs lenSq = edge.Normalize();
+			if (lenSq > bestLenSq)
+			{
+				bestLenSq = lenSq;
+				bestEdge = edge;
+				bestV0 = v0;
+				bestV1 = v1;
+			}
+			v0 = v1;
+		}
+
+		cv bestNormal;
+		cs bestLen = -FLOAT_INF;
+		for(size_t i = 0; i < face.v.size(); ++i)
+		{
+			if (face.v[i] == bestV0 || face.v[i] == bestV1)
+				continue;
+			cv edge = cv(poly.v[i]) - cv(poly.v[bestV0]);
+			edge.Normalize();
+			cv normal = bestEdge.Cross(edge);
+			cs len = normal.Normalize();
+			if (len > bestLen)
+			{
+				bestLen = len;
+				bestNormal = normal;
 			}
 		}
+		assert(bestLen != -FLOAT_INF);
+		return DIR_VEC((float)bestNormal.x, (float)bestNormal.y, (float)bestNormal.z);
+#endif
+        }
+        else if (face.v.size() == 2)
+            return DIR_TO_FLOAT4(((Vector3)poly.v[face.v[1]]-(Vector3)poly.v[face.v[0]]).Cross(((Vector3)poly.v[face.v[0]]-(Vector3)poly.v[face.v[1]]).Perpendicular()-poly.v[face.v[0]]).Normalized());
+        else if (face.v.size() == 1)
+            return cv(0, 1, 0, 0);
+        else
+            return float4::nan;
+    }
+
+    vec Polyhedron::FaceNormal(int faceIndex) const
+    {
+        cv normal = PolyFaceNormal(*this, faceIndex);
+        return DIR_VEC((float)normal.x, (float)normal.y, (float)normal.z);
+    }
+
+    bool Polyhedron::IsConvex() const
+    {
+        // This function is O(n^2).
+        /** @todo Real-Time Collision Detection, p. 64:
+            A faster O(n) approach is to compute for each face F of P the centroid C of F,
+            and for all neighboring faces G of F test if C lies behind the supporting plane of
+            G. If some C fails to lie behind the supporting plane of one or more neighboring
+            faces, P is concave, and is otherwise assumed convex. However, note that just as the
+            corresponding polygonal convexity test may fail for a pentagram this test may fail for,
+            for example, a pentagram extruded out of its plane and capped at the ends. */
+
+        float farthestD = -INFINITY;
+        int numPointsOutside = 0;
+        for(size_t i = 0; i < f.size(); ++i)
+        {
+            if (f[i].v.empty())
+                continue;
+
+            Vector3 pointOnFace = v[f[i].v[0]];
+            Vector3 faceNormal = FaceNormal((int)i);
+            for(size_t j = 0; j < v.size(); ++j)
+            {
+                float d = faceNormal.Dot(Vector3(v[j]) - pointOnFace);
+                if (d > 1e-2f)
+                {
+                    if (d > farthestD)
+                        farthestD = d;
+                    ++numPointsOutside;
+//					LOGW("Distance of vertex %s/%d from face %s/%d: %f",
+//					Vertex(j).ToString().c_str(), (int)j, FacePlane(i).ToString().c_str(), (int)i, d);
+//				return false;
+                }
+            }
+        }
+        if (numPointsOutside > 0)
+        {
+            printf("%d point-planes are outside the face planes. Farthest is at distance %f!", numPointsOutside, farthestD);
+            return false;
+        }
+        return true;
+    }
+
+    bool Polyhedron::ContainsConvex(const Vector3 &point, float epsilon) const
+    {
+        assert(IsConvex());
+        for(int i = 0; i < NumFaces(); ++i)
+            if (FacePlane(i).SignedDistance(point) > epsilon)
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::ContainsConvex(const LineSegment &lineSegment) const {
+        return ContainsConvex(lineSegment.A) && ContainsConvex(lineSegment.B);
+    }
+
+    bool Polyhedron::ContainsConvex(const Triangle &triangle) const
+    {
+        return ContainsConvex(triangle.V0) && ContainsConvex(triangle.V1) && ContainsConvex(triangle.V2);
+    }
+
 
 }