Implement Polyhedron header
This commit is contained in:
10
include/J3ML/Geometry/Line.h
Normal file
10
include/J3ML/Geometry/Line.h
Normal file
@@ -0,0 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
|
||||
namespace J3ML::Geometry
|
||||
{
|
||||
class Line
|
||||
{
|
||||
|
||||
};
|
||||
}
|
||||
@@ -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);
|
||||
};
|
||||
}
|
||||
@@ -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:
|
||||
|
||||
};
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user