diff --git a/include/J3ML/Geometry/Line.h b/include/J3ML/Geometry/Line.h new file mode 100644 index 0000000..22bbcd3 --- /dev/null +++ b/include/J3ML/Geometry/Line.h @@ -0,0 +1,10 @@ +#pragma once + + +namespace J3ML::Geometry +{ + class Line + { + + }; +} \ No newline at end of file diff --git a/include/J3ML/Geometry/Plane.h b/include/J3ML/Geometry/Plane.h index 090a914..ca2b3f6 100644 --- a/include/J3ML/Geometry/Plane.h +++ b/include/J3ML/Geometry/Plane.h @@ -1,6 +1,7 @@ #pragma once #include #include "Shape.h" +#include "Ray.h" namespace J3ML::Geometry @@ -25,5 +26,7 @@ namespace J3ML::Geometry { } + + bool Intersects(J3ML::Geometry::Ray ray, float *pDouble); }; } \ No newline at end of file diff --git a/include/J3ML/Geometry/Polyhedron.h b/include/J3ML/Geometry/Polyhedron.h index 43676c9..a373887 100644 --- a/include/J3ML/Geometry/Polyhedron.h +++ b/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 Faces() const; + + int NumEdges() const; + + LineSegment Edge(int edgeIndex) const; + + std::vector> EdgeIndices() const; + + std::vector Edges() const; + + Polygon FacePolygon(int faceIndex) const; + + bool IsConvex() const; protected: private: + }; } \ No newline at end of file diff --git a/src/J3ML/Geometry/Polyhedron.cpp b/src/J3ML/Geometry/Polyhedron.cpp index 0bbc9fe..9dc46a2 100644 --- a/src/J3ML/Geometry/Polyhedron.cpp +++ b/src/J3ML/Geometry/Polyhedron.cpp @@ -3,9 +3,20 @@ #include #include #include "J3ML/Geometry/Ray.h" +#include +#include 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 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 Polyhedron::Edges() const + { + std::vector > edges = EdgeIndices(); + std::vector 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 > Polyhedron::EdgeIndices() const + { + std::set > 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 >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 Polyhedron::Faces() const + { + std::vector 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); + } + + }