josh/j3ml
1
0
Fork 1
Code Issues 12 Pull Requests Actions Packages Projects Releases 41 Wiki Activity
Files
a3d4562decac9c2901b1f7ee0e7396e654c43e29
j3ml/src/J3ML/Geometry/Polyhedron.cpp

759 lines
28 KiB
C++
Raw Blame History

#include <J3ML/Geometry/Polyhedron.hpp>
#include <J3ML/Geometry/AABB.hpp>
#include <J3ML/Geometry/Triangle.hpp>
#include <J3ML/Geometry/LineSegment.hpp>
#include "J3ML/Geometry/Ray.hpp"
#include "J3ML/Geometry/Line.hpp"
#include <J3ML/Geometry/Polygon.hpp>
#include <J3ML/Geometry/Capsule.hpp>
#include <set>
#include <cfloat>
namespace J3ML::Geometry
{
int Polyhedron::ExtremeVertex(const Vector3 &direction) const
{
int mostExtreme = -1;
float mostExtremeDist = -FLT_MAX;
for(int i = 0; i < NumVertices(); ++i)
{
float d = Vector3::Dot(direction, Vertex(i));
if (d > mostExtremeDist)
{
mostExtremeDist = d;
mostExtreme = i;
}
}
return mostExtreme;
}
Vector3 Polyhedron::ExtremePoint(const Vector3 &direction) const
{
return Vertex(ExtremeVertex(direction));
}
void Polyhedron::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
{
///\todo Optimize!
Vector3 minPt = ExtremePoint(-direction);
Vector3 maxPt = ExtremePoint(direction);
outMin = Vector3::Dot(minPt, direction);
outMax = Vector3::Dot(maxPt, direction);
}
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();
for (int i = 0; i < NumVertices(); ++i)
aabb.Enclose(Vertex(i));
return aabb;
}
Vector3 Polyhedron::Vertex(int vertexIndex) const {
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) {
if (v.size() == 3)
return Triangle(Vector3(v[0]), Vector3(v[1]), Vector3(v[2])).Contains(point);
else if (v.size() == 2)
return LineSegment(Vector3(v[0]), Vector3(v[1])).Contains(point);
else if (v.size() == 1)
return Vector3(v[0]).Equals(point);
else
return false;
}
int bestNumIntersections = 0;
float bestFaceContainmentDistance = 0.f;
// For N > 4 Order Polyhedra
// General strategy: pick a ray from the query point to a random direction, and count the number of times the ray intersects a face.
// If it intersects an odd number of times, the given point must have been inside the polyhedron.
// But unfortunately for numerical stability, we must be smart with the choice of the ray direction. If we pick a ray direction
// which exits the polyhedron precisely at a vertex, or at an edge of two adjoining faces, we might count those twice. Therefore
// Try to pick a ray direction that passes safely through a center of some face. If we detect that there was a tricky face that
// the ray passed too close to an edge, we have no choice but to pick another ray direction and hope that it passes through
// the polyhedron in a safe manner.
// Loop through each face to choose the ray direction. If our choice was good, we only do this once and the algorithm
// after the first iteration at j == 0. If not, we iterate more faces of the polyhedron to try to find one that is safe for
// ray-polyhedron examination.
for (int j = 0; j < (int)f.size(); ++j)
{
if (f[j].v.size() < 3)
continue;
// Accumulate how many times the ray intersected a face of the polyhedron.
int numIntersections = 0;
// Track a pseudo-distance of the closest edge of a face that the ray passed through. If this distance ends up being too small,
// we decide not to trust the result we got, and proceed to another iteration of j, hoping to guess a better-behaving direction
// for the test ray.
float faceContainmentDistance = INFINITY;
Vector3 Dir = ((Vector3)v[f[j].v[0]] + (Vector3)v[f[j].v[1]] + (Vector3)v[f[j].v[2]]) * 0.333333333333f - point;
//if (Dir.Normalize() <= 0.f)
//continue;
Ray r(Vector3(), Dir);
for (int i = 0; i < (int)f.size(); ++i)
{
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 faceIndex, const Vector3 &worldSpacePoint, float polygonThickness) const
{
// N.B. This implementation is a duplicate of Polygon::Contains, but adapted to avoid dynamic memory allocation
// related to converting the face of a Polyhedron to a Polygon object.
// Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
const Face &face = f[faceIndex];
const std::vector<int> &vertices = face.v;
if (vertices.size() < 3)
return -INFINITY; // Certainly not intersecting, so return -inf denoting "strongly not contained"
Plane p = FacePlane(faceIndex);
if (FacePlane(faceIndex).Distance(worldSpacePoint) > polygonThickness)
return -INFINITY;
int numIntersections = 0;
Vector3 basisU = (Vector3)v[vertices[1]] - (Vector3)v[vertices[0]];
basisU.Normalize();
Vector3 basisV = Vector3::Cross(p.Normal, basisU).Normalized();
assert(basisU.IsNormalized());
assert(basisV.IsNormalized());
assert(basisU.IsPerpendicular(basisV));
assert(basisU.IsPerpendicular(p.Normal));
assert(basisV.IsPerpendicular(p.Normal));
// Tracks a pseudo-distance of the point to the ~nearest edge of the polygon. If the point is very close to the polygon
// edge, this is very small, and it's possible that due to numerical imprecision we cannot rely on the result in higher-level
// algorithms that invoke this function.
float faceContainmentDistance = INFINITY;
const float epsilon = 1e-4f;
Vector3 vt = Vector3(v[vertices.back()]) - worldSpacePoint;
Vector2 p0 = Vector2(Vector3::Dot(vt, basisU), Vector3::Dot(vt, basisV));
if (std::abs(p0.y) < epsilon)
p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
for(size_t i = 0; i < vertices.size(); ++i)
{
vt = Vector3(v[vertices[i]]) - worldSpacePoint;
Vector2 p1 = Vector2(Vector3::Dot(vt, basisU), Vector3::Dot(vt, basisV));
if (std::abs(p1.y) < epsilon)
p1.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
if (p0.y * p1.y < 0.f)
{
float minX = std::min(p0.x, p1.x);
if (minX > 0.f)
{
faceContainmentDistance = std::min(faceContainmentDistance, minX);
++numIntersections;
}
else if (std::max(p0.x, p1.x) > 0.f)
{
// P = p0 + t*(p1-p0) == (x,0)
// p0.x + t*(p1.x-p0.x) == x
// p0.y + t*(p1.y-p0.y) == 0
// t == -p0.y / (p1.y - p0.y)
// Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate.
Vector2 d = p1 - p0;
if (d.y != 0.f)
{
float t = -p0.y / d.y;
float x = p0.x + t * d.x;
if (t >= 0.f && t <= 1.f)
{
// Remember how close the point was to the edge, for tracking robustness/goodness of the result.
// If this is very large, then we can conclude that the point was contained or not contained in the face.
faceContainmentDistance = std::min(faceContainmentDistance, std::abs(x));
if (x >= 0.f)
++numIntersections;
}
}
}
}
p0 = p1;
}
// Positive return value: face contains the point. Negative: face did not contain the point.
return (numIntersections % 2 == 1) ? faceContainmentDistance : -faceContainmentDistance;
}
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 {
// TODO: Implement
return false;
}
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]], Vector3(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 Vector4(Vector3(((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]]).TryNormalize()));
else if (face.v.size() == 1)
return Vector4(0, 1, 0, 0);
else
return Vector4::NaN;
}
Vector3 Polyhedron::FaceNormal(int faceIndex) const
{
Vector4 normal = PolyFaceNormal(*this, faceIndex);
return Vector3((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::EulerFormulaHolds() const { return NumVertices() + NumFaces() - NumEdges() == 2;}
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);
}
bool Polyhedron::FaceContains(int faceIndex, const Vector3 &worldSpacePoint, float polygonThickness) const
{
float faceContainmentDistance = FaceContainmentDistance2D(faceIndex, worldSpacePoint, polygonThickness);
return faceContainmentDistance >= 0.f;
}
bool Polyhedron::Intersects(const LineSegment &lineSegment) const
{
if (Contains(lineSegment))
return true;
for(int i = 0; i < NumFaces(); ++i)
{
float t;
Plane plane = FacePlane(i);
bool intersects = Plane::IntersectLinePlane(plane.Normal, plane.distance, lineSegment.A, lineSegment.B - lineSegment.A, t);
if (intersects && t >= 0.f && t <= 1.f)
if (FaceContains(i, lineSegment.GetPoint(t)))
return true;
}
return false;
}
bool Polyhedron::Intersects(const Line &line) const
{
for(int i = 0; i < NumFaces(); ++i)
if (FacePolygon(i).Intersects(line))
return true;
return false;
}
bool Polyhedron::Intersects(const Ray &ray) const
{
for(int i = 0; i < NumFaces(); ++i)
if (FacePolygon(i).Intersects(ray))
return true;
return false;
}
bool Polyhedron::Intersects(const Plane &plane) const
{
return plane.Intersects(*this);
}
Vector3 Polyhedron::ApproximateConvexCentroid() const
{
// Since this shape is convex, the averaged position of all vertices is inside this polyhedron.
Vector3 arbitraryCenterVertex = Vector3::Zero;
for(int i = 0; i < NumVertices(); ++i)
arbitraryCenterVertex += Vertex(i);
return arbitraryCenterVertex / (float)NumVertices();
}
/** The algorithm for Polyhedron-Polyhedron intersection is from Christer Ericson's Real-Time Collision Detection, p. 384.
As noted by the author, the algorithm is very naive (and here unoptimized), and better methods exist. [groupSyntax] */
bool Polyhedron::Intersects(const Polyhedron &polyhedron) const
{
Vector3 c = this->ApproximateConvexCentroid();
if (polyhedron.Contains(c) && this->Contains(c))
return true;
c = polyhedron.ApproximateConvexCentroid();
if (polyhedron.Contains(c) && this->Contains(c))
return true;
// This test assumes that both this and the other polyhedron are closed.
// This means that for each edge running through vertices i and j, there's a face
// that contains the line segment (i,j) and another neighboring face that contains
// the line segment (j,i). These represent the same line segment (but in opposite direction)
// so we only have to test one of them for intersection. Take i < j as the canonical choice
// and skip the other winding order.
// Test for each edge of this polyhedron whether the other polyhedron intersects it.
for(size_t i = 0; i < f.size(); ++i)
{
assert(!f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
int v0 = f[i].v.back();
Vector3 l0 = v[v0];
for(size_t j = 0; j < f[i].v.size(); ++j)
{
int v1 = f[i].v[j];
Vector3 l1 = v[v1];
if (v0 < v1 && polyhedron.Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
return true;
l0 = l1;
v0 = v1;
}
}
// Test for each edge of the other polyhedron whether this polyhedron intersects it.
for(size_t i = 0; i < polyhedron.f.size(); ++i)
{
assert(!polyhedron.f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
int v0 = polyhedron.f[i].v.back();
Vector3 l0 = polyhedron.v[v0];
for(size_t j = 0; j < polyhedron.f[i].v.size(); ++j)
{
int v1 = polyhedron.f[i].v[j];
Vector3 l1 = polyhedron.v[v1];
if (v0 < v1 && Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
return true;
l0 = l1;
v0 = v1;
}
}
return false;
}
template <typename T>
bool PolyhedronIntersectsAABB_OBB(const Polyhedron& p, const T& obj) {
if (p.Contains(obj.CenterPoint()))
return true;
if (obj.Contains(p.ApproximateConvexCentroid())) // @bug: This is not correct for concave polyhedrons!
return true;
// Test for each edge of the AABB / OBB whether this polyhedron intersects it.
for(int i = 0; i < obj.NumEdges(); ++i)
if (p.Intersects(obj.Edge(i)))
return true;
// Test for each edge of this polyhedron whether the AABB / OBB intersects it.
for(size_t i = 0; i < p.f.size(); ++i)
{
assert(!p.f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
int v0 = p.f[i].v.back();
Vector3 l0 = p.v[v0];
for(size_t j = 0; j < p.f[i].v.size(); ++j)
{
int v1 = p.f[i].v[j];
Vector3 l1 = p.v[v1];
if (v0 < v1 && obj.Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
return true;
l0 = l1;
v0 = v1;
}
}
return false;
}
bool Polyhedron::Intersects(const AABB &aabb) const {
return PolyhedronIntersectsAABB_OBB(*this, aabb);
}
bool Polyhedron::Intersects(const OBB& obb) const {
return PolyhedronIntersectsAABB_OBB(*this, obb);
}
bool Polyhedron::Intersects(const Frustum& obb) const {
return PolyhedronIntersectsAABB_OBB(*this, obb);
}
bool Polyhedron::Intersects(const Triangle& obb) const {
return PolyhedronIntersectsAABB_OBB(*this, obb);
}
bool Polyhedron::Intersects(const Sphere& sphere) const {
Vector3 closestPt = ClosestPoint(sphere.Position);
return closestPt.DistanceSq(sphere.Position) <= sphere.Radius * sphere.Radius;
}
bool Polyhedron::Intersects(const Polygon& polygon) const {
return Intersects(polygon.ToPolyhedron());
}
bool Polyhedron::Intersects(const Capsule &capsule) const {
Vector3 pt, ptOnLineSegment;
pt = ClosestPoint(capsule.l, &ptOnLineSegment);
return pt.DistanceSq(ptOnLineSegment) <= capsule.r * capsule.r;
}
Vector3 Polyhedron::ClosestPoint(const LineSegment& lineSegment, Vector3 *lineSegmentPt) const {
if (Contains(lineSegment.A))
{
if (lineSegmentPt)
*lineSegmentPt = lineSegment.A;
return lineSegment.A;
}
if (Contains(lineSegment.B))
{
if (lineSegmentPt)
*lineSegmentPt = lineSegment.B;
return lineSegment.B;
}
Vector3 closestPt = Vector3::NaN;
float closestDistance = FLT_MAX;
Vector3 closestLineSegmentPt = Vector3::NaN;
for(int i = 0; i < NumFaces(); ++i)
{
Vector3 lineSegPt;
Vector3 pt = FacePolygon(i).ClosestPoint(lineSegment, &lineSegPt);
float d = pt.DistanceSq(lineSegPt);
if (d < closestDistance)
{
closestDistance = d;
closestPt = pt;
closestLineSegmentPt = lineSegPt;
}
}
if (lineSegmentPt)
*lineSegmentPt = closestLineSegmentPt;
return closestPt;
}
Vector3 Polyhedron::ClosestPoint(const Vector3 &point) const {
if (Contains(point))
return point;
Vector3 closestPoint = Vector3::NaN;
float closestDistance = Math::Infinity;
for (int i = 0; i < NumFaces(); ++i) {
Vector3 closestOnPoly = FacePolygon(i).ClosestPoint(point);
float d = closestOnPoly.DistanceSq(point);
if (d < closestDistance) {
closestPoint = closestOnPoly;
closestDistance = d;
}
}
return closestPoint;
}
}
Reference in New Issue View Git Blame Copy Permalink