276 lines
9.6 KiB
C++
276 lines
9.6 KiB
C++
#include <J3ML/Geometry/Plane.hpp>
|
|
#include <J3ML/Geometry/AABB.hpp>
|
|
#include <J3ML/Geometry/OBB.hpp>
|
|
#include <J3ML/Geometry/Capsule.hpp>
|
|
#include <J3ML/Geometry/Polygon.hpp>
|
|
#include <J3ML/Geometry/Sphere.hpp>
|
|
|
|
namespace J3ML::Geometry
|
|
{
|
|
|
|
bool Plane::IntersectLinePlane(const Vector3 &planeNormal, float planeD, const Vector3 &linePos, const Vector3 &lineDir, float &t)
|
|
{
|
|
/* The set of points x lying on a plane is defined by the equation
|
|
|
|
<planeNormal, x> = planeD.
|
|
|
|
The set of points x on a line is constructed explicitly by a single parameter t by
|
|
|
|
x = linePos + t*lineDir.
|
|
|
|
To solve the intersection of these two objects, substitute the second equation to the first above,
|
|
and we get
|
|
|
|
<planeNormal, linePos + t*lineDir> == planeD, or
|
|
<planeNormal, linePos> + t * <planeNormal, lineDir> == planeD, or
|
|
t == (planeD - <planeNormal, linePos>) / <planeNormal, lineDir>,
|
|
|
|
assuming that <planeNormal, lineDir> != 0.
|
|
|
|
If <planeNormal, lineDir> == 0, then the line is parallel to the plane, and either no intersection occurs, or the whole line
|
|
is embedded on the plane, and infinitely many intersections occur. */
|
|
|
|
float denom = Vector3::Dot(planeNormal, lineDir);
|
|
if (std::abs(denom) > 1e-4f)
|
|
{
|
|
// Compute the distance from the line starting point to the point of intersection.
|
|
t = (planeD - Vector3::Dot(planeNormal, linePos)) / denom;
|
|
return true;
|
|
}
|
|
|
|
if (denom != 0.f)
|
|
{
|
|
t = (planeD - Vector3::Dot(planeNormal, linePos)) / denom;
|
|
if (std::abs(t) < 1e4f)
|
|
return true;
|
|
}
|
|
t = 0.f;
|
|
return Math::EqualAbs(Vector3::Dot(planeNormal, linePos), planeD, 1e-3f);
|
|
}
|
|
|
|
template<typename T>
|
|
float Plane_SignedDistance(const Plane &plane, const T &object)
|
|
{
|
|
float pMin, pMax;
|
|
assert(plane.Normal.IsNormalized());
|
|
object.ProjectToAxis(plane.Normal, pMin, pMax);
|
|
pMin -= plane.distance;
|
|
pMax -= plane.distance;
|
|
if (pMin * pMax <= 0.f)
|
|
return 0.f;
|
|
return std::abs(pMin) < std::abs(pMax) ? pMin : pMax;
|
|
}
|
|
|
|
float Plane::SignedDistance(const Vector3 &point) const {
|
|
assert(Normal.IsNormalized());
|
|
return Normal.Dot(point) - distance;
|
|
}
|
|
|
|
float Plane::SignedDistance(const AABB &aabb) const { return Plane_SignedDistance(*this, aabb); }
|
|
|
|
float Plane::SignedDistance(const OBB &obb) const { return Plane_SignedDistance(*this, obb); }
|
|
|
|
float Plane::SignedDistance(const Capsule &capsule) const { return Plane_SignedDistance(*this, capsule); }
|
|
|
|
float Plane::SignedDistance(const Frustum &frustum) const { return Plane_SignedDistance(*this, frustum); }
|
|
|
|
float Plane::SignedDistance(const LineSegment &lineSegment) const { return Plane_SignedDistance(*this, lineSegment); }
|
|
|
|
float Plane::SignedDistance(const Ray &ray) const { return Plane_SignedDistance(*this, ray); }
|
|
|
|
float Plane::SignedDistance(const Polygon &polygon) const { return Plane_SignedDistance(*this, polygon); }
|
|
|
|
float Plane::SignedDistance(const Polyhedron &polyhedron) const { return Plane_SignedDistance(*this, polyhedron); }
|
|
|
|
float Plane::SignedDistance(const Sphere &sphere) const { return Plane_SignedDistance(*this, sphere); }
|
|
|
|
float Plane::SignedDistance(const Triangle &triangle) const { return Plane_SignedDistance(*this, triangle); }
|
|
|
|
float Plane::Distance(const Vector3 &point) const {
|
|
return std::abs(SignedDistance(point));
|
|
}
|
|
|
|
float Plane::Distance(const LineSegment &lineSegment) const
|
|
{
|
|
return lineSegment.Distance(*this);
|
|
}
|
|
|
|
float Plane::Distance(const Sphere &sphere) const
|
|
{
|
|
return std::max(0.f, Distance(sphere.Position) - sphere.Radius);
|
|
}
|
|
|
|
float Plane::Distance(const Capsule &capsule) const
|
|
{
|
|
return std::max(0.f, Distance(capsule.l) - capsule.r);
|
|
}
|
|
|
|
Plane::Plane(const Line &line, const Vector3 &normal) {
|
|
Vector3 perpNormal = normal - normal.ProjectToNorm(line.Direction);
|
|
Set(line.Position, perpNormal.Normalized());
|
|
}
|
|
|
|
void Plane::Set(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) {
|
|
Normal = (v2-v1).Cross(v3-v1);
|
|
float len = Normal.Length();
|
|
assert(len > 1e-10f);
|
|
Normal /= len;
|
|
assert(Normal.IsNormalized());
|
|
distance = Normal.Dot(v1);
|
|
}
|
|
|
|
void Plane::Set(const Vector3 &point, const Vector3 &normal_) {
|
|
Normal = normal_;
|
|
assert(Normal.IsNormalized());
|
|
distance = point.Dot(Normal);
|
|
|
|
#ifdef MATH_ASSERT_CORRECTNESS
|
|
assert1(EqualAbs(SignedDistance(point), 0.f, 0.01f), SignedDistance(point));
|
|
assert1(EqualAbs(SignedDistance(point + normal_), 1.f, 0.01f), SignedDistance(point + normal_));
|
|
#endif
|
|
}
|
|
|
|
Plane::Plane(const Vector3 &normal, float d): Normal(normal), distance(d) {}
|
|
|
|
Plane::Plane(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) {
|
|
Set(v1, v2, v3);
|
|
}
|
|
|
|
Plane::Plane(const Vector3 &pos, const Vector3 &norm) : Position(pos), Normal(norm) {}
|
|
|
|
bool Plane::Intersects(J3ML::Geometry::Ray ray, float *dist) const {
|
|
float t;
|
|
bool success = IntersectLinePlane(Normal, this->distance, ray.Origin, ray.Direction, t);
|
|
if (dist)
|
|
*dist = t;
|
|
return success && t >= 0.f;
|
|
}
|
|
|
|
bool Plane::Intersects(const Line &line, float *dist) const
|
|
{
|
|
float t;
|
|
bool intersects = IntersectLinePlane(Normal, this->distance, line.Position, line.Direction, t);
|
|
if (dist)
|
|
*dist = t;
|
|
return intersects;
|
|
}
|
|
|
|
bool Plane::Intersects(const LineSegment &lineSegment, float *dist) const
|
|
{
|
|
float t;
|
|
bool success = IntersectLinePlane(Normal, this->distance, lineSegment.A, lineSegment.Dir(), t);
|
|
const float lineSegmentLength = lineSegment.Length();
|
|
if (dist)
|
|
*dist = t / lineSegmentLength;
|
|
return success && t >= 0.f && t <= lineSegmentLength;
|
|
}
|
|
|
|
bool Plane::Intersects(const Sphere &sphere) const
|
|
{
|
|
return Distance(sphere.Position) <= sphere.Radius;
|
|
}
|
|
|
|
bool Plane::Intersects(const Capsule &capsule) const
|
|
{
|
|
return capsule.Intersects(*this);
|
|
}
|
|
|
|
/// The Plane-AABB intersection is implemented according to Christer Ericson's Real-Time Collision Detection, p.164. [groupSyntax]
|
|
bool Plane::Intersects(const AABB &aabb) const
|
|
{
|
|
Vector3 c = aabb.Centroid();
|
|
Vector3 e = aabb.HalfDiagonal();
|
|
|
|
// Compute the projection interval radius of the AABB onto L(t) = aabb.center + t * plane.normal;
|
|
float r = e[0]*std::abs(Normal[0]) + e[1]*std::abs(Normal[1]) + e[2]*std::abs(Normal[2]);
|
|
// Compute the distance of the box center from plane.
|
|
//float s = Dot(normal, c) - d;
|
|
float s = Vector3::Dot(Normal, c) - distance; ///\todo Use the above form when Plane is SSE'ized.
|
|
return std::abs(s) <= r;
|
|
}
|
|
|
|
bool Plane::Intersects(const OBB &obb) const
|
|
{
|
|
return obb.Intersects(*this);
|
|
}
|
|
|
|
bool Plane::Intersects(const Triangle &triangle) const
|
|
{
|
|
float a = SignedDistance(triangle.V0);
|
|
float b = SignedDistance(triangle.V1);
|
|
float c = SignedDistance(triangle.V2);
|
|
return (a*b <= 0.f || a*c <= 0.f);
|
|
}
|
|
|
|
bool Plane::Intersects(const Frustum &frustum) const
|
|
{
|
|
bool sign = IsOnPositiveSide(frustum.CornerPoint(0));
|
|
for(int i = 1; i < 8; ++i)
|
|
if (sign != IsOnPositiveSide(frustum.CornerPoint(i)))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
bool Plane::IsOnPositiveSide(const Vector3 &point) const {
|
|
return SignedDistance(point) >= 0.f;
|
|
}
|
|
|
|
bool Plane::Intersects(const Polyhedron &polyhedron) const
|
|
{
|
|
if (polyhedron.NumVertices() == 0)
|
|
return false;
|
|
bool sign = IsOnPositiveSide(polyhedron.Vertex(0));
|
|
for(int i = 1; i < polyhedron.NumVertices(); ++i)
|
|
if (sign != IsOnPositiveSide(polyhedron.Vertex(i)))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
Vector3 Plane::Project(const Vector3 &point) const
|
|
{
|
|
Vector3 projected = point - (Normal.Dot(point) - distance) * Normal;
|
|
return projected;
|
|
}
|
|
|
|
Vector3 Plane::ProjectToNegativeHalf(const Vector3 &point) const {
|
|
return point - Math::Max(0.f, (Normal.Dot(point) - distance)) * Normal;
|
|
}
|
|
|
|
Vector3 Plane::ProjectToPositiveHalf(const Vector3 &point) const {
|
|
return point - Math::Min(0.f, (Vector3::Dot(Normal, point) - distance)) * Normal;
|
|
}
|
|
|
|
LineSegment Plane::Project(const LineSegment &lineSegment) {
|
|
return LineSegment(Project(lineSegment.A), Project(lineSegment.B));
|
|
}
|
|
|
|
/*int Plane::Intersects(const Circle &circle, Vector3 *pt1, Vector3 *pt2) const
|
|
{
|
|
Line line;
|
|
bool planeIntersects = Intersects(circle.ContainingPlane(), &line);
|
|
if (!planeIntersects)
|
|
return false;
|
|
|
|
// Offset both line and circle position so the circle origin is at center.
|
|
line.pos -= circle.pos;
|
|
|
|
float a = 1.f;
|
|
float b = 2.f * Dot(line.pos, line.dir);
|
|
float c = line.pos.LengthSq() - circle.r * circle.r;
|
|
float r1, r2;
|
|
int numRoots = Polynomial::SolveQuadratic(a, b, c, r1, r2);
|
|
if (numRoots >= 1 && pt1)
|
|
*pt1 = circle.pos + line.GetPoint(r1);
|
|
if (numRoots >= 2 && pt2)
|
|
*pt2 = circle.pos + line.GetPoint(r2);
|
|
return numRoots;
|
|
}
|
|
|
|
int Plane::Intersects(const Circle &circle) const
|
|
{
|
|
return Intersects(circle, 0, 0);
|
|
}*/
|
|
|
|
}
|
|
|