josh/j3ml
1
0
Fork 1
Code Issues 12 Pull Requests Actions Packages Projects Releases 41 Wiki Activity

Tweeker Commit (Have Fun Reviewing This)

Browse Source
This commit is contained in:
josh
2024-04-05 12:15:01 -04:00
parent 815e914c7f
commit 0aff68b63e
30 changed files with 2261 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/J3ML/Geometry.h
View File

@@ -5,6 +5,7 @@
#include <J3ML/Geometry/AABB2D.h>
#include <J3ML/Geometry/Plane.h>
#include <J3ML/Geometry/Sphere.h>
#include <J3ML/Geometry/Line.h>
#include <J3ML/Geometry/LineSegment.h>
#include <J3ML/Geometry/Frustum.h>
#include <J3ML/Geometry/OBB.h>

4
include/J3ML/Geometry/AABB.h
View File

@@ -30,6 +30,8 @@ namespace J3ML::Geometry
AABB(const Vector3& min, const Vector3& max);
Vector3 HalfDiagonal() const { return HalfSize(); }
static int NumFaces() { return 6; }
static int NumEdges() { return 12; }
@@ -90,6 +92,8 @@ namespace J3ML::Geometry
Plane FacePlane(int faceIndex) const;
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
static AABB MinimalEnclosingAABB(const Vector3 *pointArray, int numPoints);
float GetVolume() const;
float GetSurfaceArea() const;

35
include/J3ML/Geometry/Capsule.h
View File

@@ -7,6 +7,15 @@
namespace J3ML::Geometry
{
// TODO: Move to separate math lib, find duplicates!
template <typename T>
void Swap(T &a, T &b)
{
T temp = std::move(a);
a = std::move(b);
b = std::move(temp);
}
using namespace LinearAlgebra;
class Capsule : public Shape
{
@@ -27,5 +36,31 @@ namespace J3ML::Geometry
Vector3 Centroid() const;
Vector3 ExtremePoint(const Vector3& direction);
AABB MinimalEnclosingAABB() const;
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
bool Intersects(const Plane &plane) const;
bool Intersects(const Ray &ray) const;
bool Intersects(const Line &line) const;
bool Intersects(const LineSegment &lineSegment) const;
bool Intersects(const AABB &aabb) const;
bool Intersects(const OBB &obb) const;
bool Intersects(const Sphere &sphere) const;
bool Intersects(const Capsule &capsule) const;
bool Intersects(const Triangle &triangle) const;
bool Intersects(const Polygon &polygon) const;
bool Intersects(const Frustum &frustum) const;
bool Intersects(const Polyhedron &polyhedron) const;
};
}

1
include/J3ML/Geometry/Common.h
View File

@@ -10,6 +10,7 @@ namespace J3ML::Geometry
class Capsule;
class Frustum;
class LineSegment;
class Line;
class OBB;
class Plane;
class Polygon;

4
include/J3ML/Geometry/Frustum.h
View File

@@ -207,5 +207,9 @@ namespace J3ML::Geometry
bool Intersects(const Capsule& obb) const;
bool Intersects(const Frustum& plane) const;
bool Intersects(const Polyhedron& triangle) const;
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
void GetCornerPoints(Vector3 *outPointArray) const;
};
}

14
include/J3ML/Geometry/Line.h
View File

@@ -1,10 +1,24 @@
#pragma once
#include "LineSegment.h"
namespace J3ML::Geometry
{
class Line
{
public:
Vector3 Position; /// Specifies the origin of this line.
Vector3 Direction; /// The normalized direction vector of this ray.
public:
static void
ClosestPointLineLine(const Vector3 &v0, const Vector3 &v10, const Vector3 &v2, const Vector3 &v32, float &d,
float &d2);
Vector3 ClosestPoint(const J3ML::Geometry::LineSegment &other, float &d, float &d2) const;
Vector3 GetPoint(float d) const;
Vector3 ClosestPoint(const Vector3 &targetPoint, float &d) const;
};
}

47
include/J3ML/Geometry/LineSegment.h
View File

@@ -1,9 +1,26 @@
#pragma once
#include <J3ML/LinearAlgebra/Vector3.h>
#include "Plane.h"
#include <J3ML/Geometry/Common.h>
namespace J3ML::Geometry
{
/// Clamps the given input value to the range [min, max].
/** @see Clamp01(), Min(), Max(). */
template<typename T>
T Clamp(const T &val, const T &floor, const T &ceil)
{
assert(floor <= ceil);
return val <= ceil ? (val >= floor ? val : floor) : ceil;
}
/// Clamps the given input value to the range [0, 1].
/** @see Clamp(), Min(), Max(). */
template<typename T>
T Clamp01(const T &val) { return Clamp(val, T(0), T(1)); }
using LinearAlgebra::Vector3;
class LineSegment
{
@@ -13,5 +30,35 @@ namespace J3ML::Geometry
Vector3 A;
Vector3 B;
bool Contains(const Vector3&) const;
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
Vector3 GetPoint(float d) const;
Vector3 ClosestPoint(const Vector3 &point, float &d) const;
Vector3 ClosestPoint(const Ray &other, float &d, float &d2) const;
float Distance(const Vector3 &point) const;
float DistanceSq(const Vector3& point) const;
float Distance(const Vector3 &point, float &d) const;
float Distance(const Ray &other) const;
float Distance(const Ray &other, float &d) const;
float Distance(const Ray &other, float &d, float &d2) const;
float Distance(const Line &other) const;
float Distance(const Line &other, float &d) const;
float Distance(const Line &other, float &d, float &d2) const;
float Distance(const LineSegment &other) const;
float Distance(const LineSegment &other, float &d) const;
float Distance(const LineSegment &other, float &d, float &d2) const;
float Distance(const Plane& other) const;
Vector3 Dir() const;
float Length() const;
float LengthSq() const;
float DistanceSq(const LineSegment &other) const;
Vector3 ClosestPoint(const LineSegment &other, float &d, float &d2) const;
Vector3 ClosestPoint(const Line &other, float &d, float &d2) const;
};
}

2
include/J3ML/Geometry/OBB.h
View File

@@ -94,5 +94,7 @@ namespace J3ML::Geometry {
void SetFrom(const AABB &aabb, const Matrix4x4 &transform);
void SetFrom(const AABB &aabb, const Quaternion &transform);
bool Intersects(const Plane& plane) const;
};
}

74
include/J3ML/Geometry/Plane.h
View File

@@ -11,22 +11,74 @@ namespace J3ML::Geometry
class Plane : public Shape
{
public:
Plane() : Shape() {}
Plane(const Vector3& v1, const Vector3 &v2, const Vector3& v3)
{
Set(v1, v2, v3);
}
Plane(const Vector3& pos, const Vector3& norm)
: Shape(), Position(pos), Normal(norm) {}
Vector3 Position;
Vector3 Normal;
float distance = 0.f;
public:
Plane() : Shape() {}
Plane(const Vector3& v1, const Vector3 &v2, const Vector3& v3);
Plane(const Vector3& pos, const Vector3& norm);
Plane(const Line &line, const Vector3 &normal);
void Set(const Vector3& v1, const Vector3& v2, const Vector3& v3)
{
void Set(const Vector3& v1, const Vector3& v2, const Vector3& v3);
}
void Set(const Vector3 &point, const Vector3 &normal_);
bool Intersects(J3ML::Geometry::Ray ray, float *pDouble);
bool Intersects(J3ML::Geometry::Ray ray, float *dist) const;
/// Tests if the given point lies on the positive side of this plane.
/** A plane divides the space in three sets: the negative halfspace, the plane itself, and the positive halfspace.
The normal vector of the plane points towards the positive halfspace.
@return This function returns true if the given point lies either on this plane itself, or in the positive
halfspace of this plane.
@see IsInPositiveDirection, AreOnSameSide(), Distance(), SignedDistance(). */
bool IsOnPositiveSide(const Vector3 &point) const;
float SignedDistance(const Vector3 &point) const;
float SignedDistance(const AABB &aabb) const;
float SignedDistance(const OBB &obb) const;
float SignedDistance(const Capsule &capsule) const;
//float Plane::SignedDistance(const Circle &circle) const { return Plane_SignedDistance(*this, circle); }
float SignedDistance(const Frustum &frustum) const;
//float SignedDistance(const Line &line) const { return Plane_SignedDistance(*this, line); }
float SignedDistance(const LineSegment &lineSegment) const;
float SignedDistance(const Ray &ray) const;
//float Plane::SignedDistance(const Plane &plane) const { return Plane_SignedDistance(*this, plane); }
float SignedDistance(const Polygon &polygon) const;
float SignedDistance(const Polyhedron &polyhedron) const;
float SignedDistance(const Sphere &sphere) const;
float SignedDistance(const Triangle &triangle) const;
static bool
IntersectLinePlane(const Vector3 &planeNormal, float planeD, const Vector3 &linePos, const Vector3 &lineDir,
float &t);
float Distance(const Vector3 &) const;
float Distance(const LineSegment &lineSegment) const;
float Distance(const Sphere &sphere) const;
float Distance(const Capsule &capsule) const;
bool Intersects(const Line &line, float *dist) const;
bool Intersects(const LineSegment &lineSegment, float *dist) const;
bool Intersects(const Sphere &sphere) const;
bool Intersects(const Capsule &capsule) const;
bool Intersects(const AABB &aabb) const;
bool Intersects(const OBB &obb) const;
bool Intersects(const Triangle &triangle) const;
bool Intersects(const Frustum &frustum) const;
bool Intersects(const Polyhedron &polyhedron) const;
LineSegment Project(const LineSegment &segment);
};
}

57
include/J3ML/Geometry/Polygon.h
View File

@@ -12,17 +12,52 @@ namespace J3ML::Geometry {
public:
std::vector<Vector3> vertices;
AABB MinimalEnclosingAABB() const;
int NumVertices() const
{
return (int)vertices.size();
}
Vector3 Vertex(int vertexIndex) const
{
assert(vertexIndex >= 0);
assert(vertexIndex < (int) vertices.size());
return vertices[vertexIndex];
}
int NumVertices() const;
Vector3 Vertex(int vertexIndex) const;
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
Vector3 ExtremePoint(const Vector3 &direction) const;
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
bool Intersects(const Capsule &capsule) const;
bool Intersects(const Line &line) const;
bool Intersects(const Ray &ray) const;
bool Intersects(const LineSegment &lineSegment) const;
bool Intersects(const Plane &plane) const;
bool Intersects(const AABB &aabb) const;
bool Intersects(const OBB &obb) const;
bool Intersects(const Triangle &triangle, float polygonThickness = 1e-3f) const;
bool Intersects(const Polygon &polygon, float polygonThickness = 1e-3f) const;
bool Intersects(const Frustum &frustum) const;
bool Intersects(const Polyhedron &polyhedron) const;
bool Intersects(const Sphere &sphere) const;
protected:
private:
std::vector<Triangle> Triangulate() const;
/// Tests if this polygon is planar.
/** A polygon is planar if all its vertices lie on the same plane.
@note Almost all functions in this class require that the polygon is planar. While you can store vertices of
non-planar polygons in this class, they are better avoided. Read the member function documentation carefully
to avoid calling for non-planar polygons any functions which assume planarity.
@see IsConvex(), IsSimple(), IsNull(), IsFinite(), IsDegenerate(). */
bool IsPlanar(float epsilonSq = 1e-4f) const;
Vector2 MapTo2D(int i) const;
Vector2 MapTo2D(const Vector3 &point) const;
Vector3 BasisU() const;
Vector3 BasisV() const;
Plane PlaneCCW() const;
bool Contains(const Polygon &worldSpacePolygon, float polygonThickness) const;
bool Contains(const Vector3 &worldSpacePoint, float polygonThicknessSq) const;
bool Contains(const LineSegment &worldSpaceLineSegment, float polygonThickness) const;
};
}

23
include/J3ML/Geometry/Polyhedron.h
View File

@@ -40,7 +40,7 @@ namespace J3ML::Geometry
Vector3 Vertex(int vertexIndex) const;
bool Intersects(const LineSegment&) const;
bool Contains(const Vector3&) const;
bool Contains(const LineSegment&) const;
@@ -55,6 +55,8 @@ namespace J3ML::Geometry
bool ContainsConvex(const LineSegment&) const;
bool ContainsConvex(const Triangle&) const;
bool Intersects(const Line&) const;
bool Intersects(const LineSegment&) const;
bool Intersects(const Ray&) const;
bool Intersects(const Plane&) const;
bool Intersects(const Polyhedron&) const;
@@ -84,9 +86,28 @@ namespace J3ML::Geometry
Polygon FacePolygon(int faceIndex) const;
Vector3 FaceNormal(int faceIndex) const;
bool IsConvex() const;
Vector3 ApproximateConvexCentroid() const;
int ExtremeVertex(const Vector3 &direction) const;
Vector3 ExtremePoint(const Vector3 &direction) const;
/// Tests if the given face of this Polyhedron contains the given point.
bool FaceContains(int faceIndex, const Vector3 &worldSpacePoint, float polygonThickness = 1e-3f) const;
/// A helper for Contains() and FaceContains() tests: Returns a positive value if the given point is contained in the given face,
/// and a negative value if the given point is outside the face. The magnitude of the return value reports a pseudo-distance
/// from the point to the nearest edge of the face polygon. This is used as a robustness/stability criterion to estimate how
/// numerically believable the result is.
float FaceContainmentDistance2D(int faceIndex, const Vector3 &worldSpacePoint, float polygonThickness) const;
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
protected:
private:
};
}

6
include/J3ML/Geometry/Ray.h
View File

@@ -56,5 +56,11 @@ namespace J3ML::Geometry
// the surface intersection point,
// and the surface normal at the point of intersection.
RaycastResult Cast(std::vector<Shape> shapes, float maxDistance = 99999999);
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
Vector3 ClosestPoint(const LineSegment&, float &d, float &d2) const;
Vector3 ClosestPoint(const Vector3 &targetPoint, float &d) const;
};
}

6
include/J3ML/Geometry/Sphere.h
View File

@@ -60,12 +60,10 @@ namespace J3ML::Geometry
{
return Position.DistanceSquared(point) <= Radius*Radius + epsilon;
}
bool Contains(const LineSegment& lineseg) const
{
}
bool Contains(const LineSegment& lineseg) const;
TriangleMesh GenerateUVSphere() const {}
TriangleMesh GenerateIcososphere() const {}
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
};
}

70
include/J3ML/Geometry/Triangle.h
View File

@@ -13,9 +13,79 @@ namespace J3ML::Geometry
Vector3 V2;
bool Intersects(const AABB& aabb) const;
bool Intersects(const Capsule& capsule) const;
AABB BoundingAABB() const;
bool Contains(const Vector3&) const;
void ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const;
static float IntersectLineTri(const Vector3 &linePos, const Vector3 &lineDir, const Vector3 &v0, const Vector3 &v1,
const Vector3 &v2, float &u, float &v);
/// Computes the closest point on the edge of this triangle to the given object.
/** @param outU [out] If specified, receives the barycentric U coordinate of the returned point (in the UV convention).
This pointer may be null.
@param outV [out] If specified, receives the barycentric V coordinate of the returned point (in the UV convention).
This pointer may be null.
@param outD [out] If specified, receives the distance along the line of the closest point on the line to the edge of this triangle.
@return The closest point on the edge of this triangle to the given object.
@todo Add ClosestPointToTriangleEdge(Point/Ray/Triangle/Plane/Polygon/Circle/Disk/AABB/OBB/Sphere/Capsule/Frustum/Polyhedron).
@see Distance(), Contains(), Intersects(), ClosestPointToTriangleEdge(), Line::GetPoint. */
Vector3 ClosestPointToTriangleEdge(const Line &line, float *outU, float *outV, float *outD) const;
Vector3 ClosestPointToTriangleEdge(const LineSegment &lineSegment, float *outU, float *outV, float *outD) const;
Vector3 ClosestPoint(const LineSegment &lineSegment, Vector3 *otherPt = 0) const;
/// Returns the point at the given barycentric coordinates.
/** This function computes the vector space point at the given barycentric coordinates.
@param uvw The barycentric UVW coordinate triplet. The condition u+v+w == 1 should hold for the input coordinate.
If 0 <= u,v,w <= 1, the returned point lies inside this triangle.
@return u*a + v*b + w*c. */
Vector3 Point(const Vector3 &uvw) const;
Vector3 Point(float u, float v, float w) const;
/** These functions are an alternate form of Point(u,v,w) for the case when the barycentric coordinates are
represented as a (u,v) pair and not as a (u,v,w) triplet. This function is provided for convenience
and effectively just computes Point(1-u-v, u, v).
@param uv The barycentric UV coordinates. If 0 <= u,v <= 1 and u+v <= 1, then the returned point lies inside
this triangle.
@return a + (b-a)*u + (c-a)*v.
@see BarycentricUV(), BarycentricUVW(), BarycentricInsideTriangle(). */
Vector3 Point(const Vector2 &uv) const;
Vector3 Point(float u, float v) const;
/// Expresses the given point in barycentric (u,v,w) coordinates.
/** @note There are two different conventions for representing barycentric coordinates. One uses
a (u,v,w) triplet with the equation pt == u*a + v*b + w*c, and the other uses a (u,v) pair
with the equation pt == a + u*(b-a) + v*(c-a). These two are equivalent. Use the mappings
(u,v) -> (1-u-v, u, v) and (u,v,w)->(v,w) to convert between these two representations.
@param point The point of the vector space to express in barycentric coordinates. This point should
lie in the plane formed by this triangle.
@return The factors (u,v,w) that satisfy the weighted sum equation point == u*a + v*b + w*c.
@see BarycentricUV(), BarycentricInsideTriangle(), Point(), http://mathworld.wolfram.com/BarycentricCoordinates.html */
Vector3 BarycentricUVW(const Vector3 &point) const;
/// Expresses the given point in barycentric (u,v) coordinates.
/** @note There are two different conventions for representing barycentric coordinates. One uses
a (u,v,w) triplet with the equation pt == u*a + v*b + w*c, and the other uses a (u,v) pair
with the equation pt == a + u*(b-a) + v*(c-a). These two are equivalent. Use the mappings
(u,v) -> (1-u-v, u, v) and (u,v,w)->(v,w) to convert between these two representations.
@param point The point to express in barycentric coordinates. This point should lie in the plane
formed by this triangle.
@return The factors (u,v) that satisfy the weighted sum equation point == a + u*(b-a) + v*(c-a).
@see BarycentricUVW(), BarycentricInsideTriangle(), Point(). */
Vector2 BarycentricUV(const Vector3 &point) const;
Vector3 ClosestPoint(const Vector3 &p) const;
Plane PlaneCCW() const;
Plane PlaneCW() const;
Vector3 Vertex(int i) const;
LineSegment Edge(int i) const;
};

17
include/J3ML/LinearAlgebra/Vector2.h
View File

@@ -10,9 +10,11 @@ namespace J3ML::LinearAlgebra {
/// A 2D (x, y) ordered pair.
class Vector2 {
public:
float x = 0;
float y = 0;
public:
enum {Dimensions = 2};
public:
/// Default Constructor - Initializes values to zero
Vector2();
/// Constructs a new Vector2 with the value (X, Y)
@@ -174,9 +176,14 @@ namespace J3ML::LinearAlgebra {
Vector2& operator*=(float scalar);
Vector2& operator/=(float scalar);
public:
float x = 0;
float y = 0;
/// Tests if the triangle a->b->c is oriented counter-clockwise.
/** Returns true if the triangle a->b->c is oriented counter-clockwise, when viewed in the XY-plane
where x spans to the right and y spans up.
Another way to think of this is that this function returns true, if the point C lies to the left
of the directed line AB. */
static bool OrientedCCW(const Vector2 &, const Vector2 &, const Vector2 &);
};

5
include/J3ML/LinearAlgebra/Vector3.h
View File

@@ -145,6 +145,8 @@ public:
//float DIstance(const Triangle&) const;
float DistanceSquared(const Vector3& to) const;
// Function Alias for DistanceSquared
float DistanceSq(const Vector3& to) const { return DistanceSquared(to); }
static float DistanceSquared(const Vector3& from, const Vector3& to);
float Length() const;
@@ -183,7 +185,6 @@ public:
Vector3 Lerp(const Vector3& goal, float alpha) const;
static Vector3 Lerp(const Vector3& lhs, const Vector3& rhs, float alpha);
/// Returns the angle between this vector and the specified vector, in radians.
/** @note This function takes into account that this vector or the other vector can be unnormalized, and normalizes the computations.
If you are computing the angle between two normalized vectors, it is better to use AngleBetweenNorm().
@@ -232,13 +233,13 @@ public:
bool Equals(const Vector3& rhs, float epsilon = 1e-3f) const;
bool Equals(float _x, float _y, float _z, float epsilon = 1e-3f) const;
Vector3 &operator =(const Vector3& rhs);
Vector3& operator+=(const Vector3& rhs);
Vector3& operator-=(const Vector3& rhs);
Vector3& operator*=(float scalar);
Vector3& operator/=(float scalar);
void Set(float d, float d1, float d2);
};
static Vector3 operator*(float lhs, const Vector3& rhs)

13
src/J3ML/Geometry/AABB.cpp
View File

@@ -619,4 +619,17 @@ namespace J3ML::Geometry {
}
void AABB::ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const {
Vector3 c = (minPoint + maxPoint) * 0.5f;
Vector3 e = maxPoint - c;
// Compute the projection interval radius of the AABB onto L(t) = aabb.center + t * plane.normal;
float r = std::abs(e[0]*std::abs(axis[0]) + e[1]*std::abs(axis[1]) + e[2]*std::abs(axis[2]));
// Compute the distance of the box center from plane.
float s = axis.Dot(c);
dMin = s - r;
dMax = s + r;
}
}

80
src/J3ML/Geometry/Capsule.cpp
View File

@@ -1,5 +1,7 @@
#include <J3ML/Geometry/Capsule.h>
#include <J3ML/Geometry/AABB.h>
#include <J3ML/Geometry/Sphere.h>
#include <J3ML/Geometry/Polygon.h>
namespace J3ML::Geometry
{
@@ -12,4 +14,82 @@ namespace J3ML::Geometry
AABB aabb(Vector3::Min(l.A, l.B) - d, Vector3::Max(l.A, l.B) + d);
return aabb;
}
void Capsule::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const {
outMin = Vector3::Dot(direction, l.A);
outMax = Vector3::Dot(direction, l.B);
if (outMax < outMin)
Swap(outMin, outMax);
// The following requires that direction is normalized, otherwise we would have to sub/add 'r * direction.Length()', but
// don't want to do that for performance reasons.
assert(direction.IsNormalized());
outMin -= r;
outMax += r;
}
bool Capsule::Intersects(const Ray &ray) const
{
return l.Distance(ray) <= r;
}
bool Capsule::Intersects(const Line &line) const
{
return l.Distance(line) <= r;
}
bool Capsule::Intersects(const LineSegment &lineSegment) const
{
return l.Distance(lineSegment) <= r;
}
bool Capsule::Intersects(const Plane &plane) const {
return l.Distance(plane) <= r;
}
bool Capsule::Intersects(const AABB &aabb) const
{
//return FloatingPointOffsetedGJKIntersect(*this, aabb);
}
bool Capsule::Intersects(const OBB &obb) const
{
//return GJKIntersect(*this, obb);
}
/// [groupSyntax]
bool Capsule::Intersects(const Sphere &sphere) const
{
float R = r + sphere.Radius;
return l.DistanceSq(sphere.Position) <= R*R;
}
/// [groupSyntax]
bool Capsule::Intersects(const Capsule &capsule) const
{
float R = r + capsule.r;
return l.DistanceSq(capsule.l) <= R*R;
}
bool Capsule::Intersects(const Triangle &triangle) const
{
Vector3 thisPoint;
Vector3 trianglePoint = triangle.ClosestPoint(l, &thisPoint);
return thisPoint.DistanceSq(trianglePoint) <= r*r;
}
bool Capsule::Intersects(const Polygon &polygon) const
{
return polygon.Intersects(*this);
}
bool Capsule::Intersects(const Frustum &frustum) const
{
return frustum.Intersects(*this);
}
bool Capsule::Intersects(const Polyhedron &polyhedron) const
{
return polyhedron.Intersects(*this);
}
}

65
src/J3ML/Geometry/Frustum.cpp
View File

@@ -6,6 +6,71 @@
namespace J3ML::Geometry
{
void Frustum::GetCornerPoints(Vector3 *outPointArray) const
{
assert(outPointArray);
if (type == FrustumType::Perspective)
{
float tanhfov = std::tan(horizontalFov*0.5f);
float tanvfov = std::tan(verticalFov*0.5f);
float frontPlaneHalfWidth = tanhfov*nearPlaneDistance;
float frontPlaneHalfHeight = tanvfov*nearPlaneDistance;
float farPlaneHalfWidth = tanhfov*farPlaneDistance;
float farPlaneHalfHeight = tanvfov*farPlaneDistance;
Vector3 right = WorldRight();
Vector3 nearCenter = pos + front * nearPlaneDistance;
Vector3 nearHalfWidth = frontPlaneHalfWidth*right;
Vector3 nearHalfHeight = frontPlaneHalfHeight*up;
outPointArray[0] = nearCenter - nearHalfWidth - nearHalfHeight;
outPointArray[1] = nearCenter + nearHalfWidth - nearHalfHeight;
outPointArray[2] = nearCenter - nearHalfWidth + nearHalfHeight;
outPointArray[3] = nearCenter + nearHalfWidth + nearHalfHeight;
Vector3 farCenter = pos + front * farPlaneDistance;
Vector3 farHalfWidth = farPlaneHalfWidth*right;
Vector3 farHalfHeight = farPlaneHalfHeight*up;
outPointArray[4] = farCenter - farHalfWidth - farHalfHeight;
outPointArray[5] = farCenter + farHalfWidth - farHalfHeight;
outPointArray[6] = farCenter - farHalfWidth + farHalfHeight;
outPointArray[7] = farCenter + farHalfWidth + farHalfHeight;
}
else
{
Vector3 right = WorldRight();
Vector3 nearCenter = pos + front * nearPlaneDistance;
Vector3 farCenter = pos + front * farPlaneDistance;
Vector3 halfWidth = orthographicWidth * 0.5f * right;
Vector3 halfHeight = orthographicHeight * 0.5f * up;
outPointArray[0] = nearCenter - halfWidth - halfHeight;
outPointArray[1] = nearCenter + halfWidth - halfHeight;
outPointArray[2] = nearCenter - halfWidth + halfHeight;
outPointArray[3] = nearCenter + halfWidth + halfHeight;
outPointArray[4] = farCenter - halfWidth - halfHeight;
outPointArray[5] = farCenter + halfWidth - halfHeight;
outPointArray[6] = farCenter - halfWidth + halfHeight;
outPointArray[7] = farCenter + halfWidth + halfHeight;
}
}
void Frustum::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
{
Vector3 corners[8];
GetCornerPoints(corners);
outMax = -INFINITY;
outMin = INFINITY;
for(int i = 0; i < 8; ++i)
{
float d = Vector3::Dot(direction, corners[i]);
outMax = std::max(outMax, d);
outMin = std::min(outMin, d);
}
}
Frustum Frustum::CreateFrustumFromCamera(const CoordinateFrame &cam, float aspect, float fovY, float zNear, float zFar) {
Frustum frustum;
const float halfVSide = zFar * tanf(fovY * 0.5f);

69
src/J3ML/Geometry/Line.cpp Normal file
View File

@@ -0,0 +1,69 @@
#include <J3ML/LinearAlgebra.h>
#include <J3ML/Geometry/Line.h>
#include <J3ML/Geometry/LineSegment.h>
namespace J3ML::Geometry
{
/// Computes the closest point pair on two lines.
/** The first line is specified by two points start0 and end0. The second line is specified by two points start1 and end1.
The implementation of this function follows http://paulbourke.net/geometry/lineline3d/ .
@param v0 The starting point of the first line.
@param v10 The direction vector of the first line. This can be unnormalized.
@param v2 The starting point of the second line.
@param v32 The direction vector of the second line. This can be unnormalized.
@param d [out] Receives the normalized distance of the closest point along the first line.
@param d2 [out] Receives the normalized distance of the closest point along the second line.
@return Returns the closest point on line start0<->end0 to the second line.
@note This is a low-level utility function. You probably want to use ClosestPoint() or Distance() instead.
@see ClosestPoint(), Distance(). */
void Line::ClosestPointLineLine(const Vector3 &v0, const Vector3 &v10, const Vector3 &v2, const Vector3 &v32, float &d, float &d2)
{
assert(!v10.IsZero());
assert(!v32.IsZero());
Vector3 v02 = v0 - v2;
float d0232 = v02.Dot(v32);
float d3210 = v32.Dot(v10);
float d3232 = v32.Dot(v32);
assert(d3232 != 0.f); // Don't call with a zero direction vector.
float d0210 = v02.Dot(v10);
float d1010 = v10.Dot(v10);
float denom = d1010*d3232 - d3210*d3210;
if (denom != 0.f)
d = (d0232*d3210 - d0210*d3232) / denom;
else
d = 0.f;
d2 = (d0232 + d * d3210) / d3232;
}
Vector3 Line::GetPoint(float d) const
{
assert(Direction.IsNormalized());
return Position + d * Direction;
}
Vector3 Line::ClosestPoint(const Vector3 &targetPoint, float &d) const
{
d = Vector3::Dot(targetPoint - Position, Direction);
return GetPoint(d);
}
Vector3 Line::ClosestPoint(const LineSegment &other, float &d, float &d2) const
{
ClosestPointLineLine(Position, Direction, other.A, other.B - other.A, d, d2);
if (d2 < 0.f)
{
d2 = 0.f;
return ClosestPoint(other.A, d);
}
else if (d2 > 1.f)
{
d2 = 1.f;
return ClosestPoint(other.B, d);
}
else
return GetPoint(d);
}
}

200
src/J3ML/Geometry/LineSegment.cpp
View File

@@ -1,4 +1,6 @@
#include <J3ML/Geometry/LineSegment.h>
#include "J3ML/Geometry/Capsule.h"
#include <J3ML/Geometry/Line.h>
namespace J3ML::Geometry {
@@ -8,4 +10,202 @@ namespace J3ML::Geometry {
}
LineSegment::LineSegment() {}
void LineSegment::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
{
outMin = Vector3::Dot(direction, A);
outMax = Vector3::Dot(direction, B);
if (outMax < outMin)
Swap(outMin, outMax);
}
Vector3 LineSegment::GetPoint(float d) const
{
return (1.f - d) * A + d * B;
}
float LineSegment::Distance(const Vector3 &point, float &d) const
{
/// See Christer Ericson's Real-Time Collision Detection, p.130.
Vector3 closestPoint = ClosestPoint(point, d);
return closestPoint.Distance(point);
}
float LineSegment::DistanceSq(const Vector3 &point) const
{
float d;
/// See Christer Ericson's Real-Time Collision Detection, p.130.
Vector3 closestPoint = ClosestPoint(point, d);
return closestPoint.DistanceSq(point);
}
float LineSegment::DistanceSq(const LineSegment &other) const
{
float d, d2;
ClosestPoint(other, d, d2);
return GetPoint(d).DistanceSq(other.GetPoint(d2));
}
float LineSegment::Distance(const Plane &other) const {
{
float aDist = other.SignedDistance(A);
float bDist = other.SignedDistance(B);
if (aDist * bDist < 0.f)
return 0.f; // There was an intersection, so the distance is zero.
return std::min(std::abs(aDist), std::abs(bDist));
}
}
float LineSegment::Distance(const LineSegment &other, float &d) const { float d2; return Distance(other, d, d2); }
float LineSegment::Distance(const LineSegment &other) const { float d, d2; return Distance(other, d, d2); }
float LineSegment::Distance(const Line &other, float &d) const { float d2; return Distance(other, d, d2); }
float LineSegment::Distance(const Line &other) const { float d, d2; return Distance(other, d, d2); }
float LineSegment::Distance(const Ray &other, float &d) const { float d2; return Distance(other, d, d2); }
float LineSegment::Distance(const Ray &other) const { float d, d2; return Distance(other, d, d2); }
float LineSegment::Distance(const Vector3 &point) const { float d; return Distance(point, d); }
Vector3 LineSegment::ClosestPoint(const Vector3 &point, float &d) const {
Vector3 dir = B - A;
d = Clamp01(Vector3::Dot(point - A, dir) / dir.LengthSquared());
return A + d * dir;
}
Vector3 LineSegment::ClosestPoint(const Line &other, float &d, float &d2) const
{
Line::ClosestPointLineLine(other.Position, other.Direction, A,B - A, d2, d);
if (d < 0.f)
{
d = 0.f;
other.ClosestPoint(A, d2);
return A;
}
else if (d > 1.f)
{
d = 1.f;
other.ClosestPoint(B, d2);
return B;
}
else
return GetPoint(d);
}
Vector3 LineSegment::ClosestPoint(const Ray &other, float &d, float &d2) const {
other.ClosestPoint(*this, d2, d);
return GetPoint(d);
}
Vector3 LineSegment::ClosestPoint(const LineSegment &other, float &d, float &d2) const
{
Vector3 dir = B - A;
Line::ClosestPointLineLine(A, B - A, other.A, other.B - other.A, d, d2);
if (d >= 0.f && d <= 1.f && d2 >= 0.f && d2 <= 1.f)
return A + d * dir;
else if (d >= 0.f && d <= 1.f) // Only d2 is out of bounds.
{
Vector3 p;
if (d2 < 0.f)
{
d2 = 0.f;
p = other.A;
}
else
{
d2 = 1.f;
p = other.B;
}
return ClosestPoint(p, d);
}
else if (d2 >= 0.f && d2 <= 1.f) // Only d is out of bounds.
{
Vector3 p;
if (d < 0.f)
{
d = 0.f;
p = A;
}
else
{
d = 1.f;
p = B;
}
other.ClosestPoint(p, d2);
return p;
}
else // Both u and u2 are out of bounds.
{
Vector3 p;
if (d < 0.f)
{
p = A;
d = 0.f;
}
else
{
p = B;
d = 1.f;
}
Vector3 p2;
if (d2 < 0.f)
{
p2 = other.A;
d2 = 0.f;
}
else
{
p2 = other.B;
d2 = 1.f;
}
float T, T2;
Vector3 closestPoint = ClosestPoint(p2, T);
Vector3 closestPoint2 = other.ClosestPoint(p, T2);
if (closestPoint.DistanceSq(p2) <= closestPoint2.DistanceSq(p))
{
d = T;
return closestPoint;
}
else
{
d2 = T2;
return p;
}
}
}
float LineSegment::Distance(const Ray &other, float &d, float &d2) const {
{
ClosestPoint(other, d, d2);
return GetPoint(d).Distance(other.GetPoint(d2));
}
}
float LineSegment::Distance(const Line &other, float &d, float &d2) const {
Vector3 closestPoint2 = other.ClosestPoint(*this, d, d2);
Vector3 closestPoint = GetPoint(d2);
return closestPoint.Distance(closestPoint2);
}
Vector3 LineSegment::Dir() const {
return (B - A).Normalize();
}
float LineSegment::Length() const {
return A.Distance(B);
}
float LineSegment::LengthSq() const
{
return A.DistanceSq(B);
}
}

11
src/J3ML/Geometry/OBB.cpp
View File

@@ -379,5 +379,16 @@ namespace J3ML::Geometry
OBBTransform(*this, transform.ToMatrix3x3());
}
/// The implementation of OBB-Plane intersection test follows Christer Ericson's Real-Time Collision Detection, p. 163.
bool OBB::Intersects(const Plane& plane) const {
// Compute the projection interval radius of this OBB onto L(t) = this->pos + x * p.normal;
float t = r[0] * std::abs(Vector3::Dot(plane.Normal, axis[0])) +
r[1] * std::abs(Vector3::Dot(plane.Normal, axis[1])) +
r[2] * std::abs(Vector3::Dot(plane.Normal, axis[2]));
// Compute the distance of this OBB center from the plane.
float s = Vector3::Dot(plane.Normal, pos) - plane.distance;
return std::abs(s) <= t;
}
}

259
src/J3ML/Geometry/Plane.cpp
View File

@@ -1 +1,260 @@
#include <J3ML/Geometry/Plane.h>
#include <J3ML/Geometry/AABB.h>
#include <J3ML/Geometry/OBB.h>
#include <J3ML/Geometry/Capsule.h>
#include <J3ML/Geometry/Polygon.h>
#include <J3ML/Geometry/Sphere.h>
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 {
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.Normalize());
}
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 &v1, const Vector3 &v2, const Vector3 &v3) {
Set(v1, v2, v3);
}
Plane::Plane(const Vector3 &pos, const Vector3 &norm)
: Shape(), 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;
}
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);
}*/
}

565
src/J3ML/Geometry/Polygon.cpp
View File

@@ -1,5 +1,8 @@
#include <J3ML/Geometry/Polygon.h>
#include <J3ML/Geometry/AABB.h>
#include <J3ML/Geometry/Triangle.h>
#include "J3ML/Geometry/Plane.h"
#include "J3ML/Geometry/Line.h"
namespace J3ML::Geometry {
AABB Polygon::MinimalEnclosingAABB() const {
@@ -9,6 +12,568 @@ namespace J3ML::Geometry {
aabb.Enclose(Vertex(i));
return aabb;
}
Vector3 Polygon::ExtremePoint(const Vector3 &direction, float &projectionDistance) const
{
Vector3 mostExtreme = Vector3::NaN;
projectionDistance = -INFINITY;
for(int i = 0; i < NumVertices(); ++i)
{
Vector3 pt = Vertex(i);
float d = Vector3::Dot(direction, pt);
if (d > projectionDistance)
{
projectionDistance = d;
mostExtreme = pt;
}
}
return mostExtreme;
}
Vector3 Polygon::ExtremePoint(const Vector3 &direction) const
{
float projectionDistance;
return ExtremePoint(direction, projectionDistance);
}
void Polygon::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);
}
Vector3 Polygon::Vertex(int vertexIndex) const {
assert(vertexIndex >= 0);
assert(vertexIndex < (int) vertices.size());
return vertices[vertexIndex];
}
int Polygon::NumVertices() const {
return (int)vertices.size();
}
bool Polygon::IsPlanar(float epsilonSq) const
{
if (vertices.empty())
return false;
if (vertices.size() <= 3)
return true;
Vector3 normal = (Vector3(vertices[1])-Vector3(vertices[0])).Cross(Vector3(vertices[2])-Vector3(vertices[0]));
float lenSq = normal.LengthSquared();
for(size_t i = 3; i < vertices.size(); ++i)
{
float d = normal.Dot(Vector3(vertices[i])-Vector3(vertices[0]));
if (d*d > epsilonSq * lenSq)
return false;
}
return true;
}
Vector3 Polygon::BasisU() const
{
if (vertices.size() < 2)
return Vector3::Right;
Vector3 u = (Vector3)vertices[1] - (Vector3)vertices[0];
u = u.Normalize(); // Always succeeds, even if u was zero (generates (1,0,0)).
return u;
}
Vector3 Polygon::BasisV() const
{
if (vertices.size() < 2)
return Vector3::Down;
return Vector3::Cross(PlaneCCW().Normal, BasisU()).Normalize();
}
Plane Polygon::PlaneCCW() const
{
if (vertices.size() > 3)
{
Plane plane;
for(size_t i = 0; i < vertices.size()-2; ++i)
for(size_t j = i+1; j < vertices.size()-1; ++j)
{
Vector3 pij = Vector3(vertices[j])-Vector3(vertices[i]);
for(size_t k = j+1; k < vertices.size(); ++k)
{
plane.Normal = pij.Cross(Vector3(vertices[k])-Vector3(vertices[i]));
float lenSq = plane.Normal.LengthSquared();
if (lenSq > 1e-8f)
{
plane.Normal /= std::sqrt(lenSq);
plane.distance = plane.Normal.Dot(Vector3(vertices[i]));
return plane;
}
}
}
// warn("Polygon contains %d points, but they are all collinear! Cannot form a plane for the Polygon using three points!", (int)p.size());
// Polygon contains multiple points, but they are all collinear.
// Pick an arbitrary plane along the line as the polygon plane (as if the polygon had only two points)
Vector3 dir = (Vector3(vertices[1])-Vector3(vertices[0])).Normalize();
return Plane(Line(vertices[0], dir), dir.Perpendicular());
}
if (vertices.size() == 3)
return Plane(vertices[0], vertices[1], vertices[2]);
if (vertices.size() == 2)
{
Vector3 dir = (Vector3(vertices[1])-Vector3(vertices[0])).Normalize();
return Plane(Line(vertices[0], dir), dir.Perpendicular());
}
if (vertices.size() == 1)
return Plane(vertices[0], Vector3(0,1,0));
return Plane();
}
Vector2 Polygon::MapTo2D(int i) const
{
assert(i >= 0);
assert(i < (int)vertices.size());
return MapTo2D(vertices[i]);
}
Vector2 Polygon::MapTo2D(const Vector3 &point) const
{
assert(!vertices.empty());
Vector3 basisU = BasisU();
Vector3 basisV = BasisV();
Vector3 pt = point - vertices[0];
return Vector2(Vector3::Dot(pt, basisU), Vector3::Dot(pt, basisV));
}
// A(u) = a1 + u * (a2-a1).
// B(v) = b1 + v * (b2-b1).
// Returns (u,v).
bool IntersectLineLine2D(const Vector2 &a1, const Vector2 &a2, const Vector2 &b1, const Vector2 &b2, Vector2 &out)
{
float u = (b2.x - b1.x)*(a1.y - b1.y) - (b2.y - b1.y)*(a1.x - b1.x);
float v = (a2.x - a1.x)*(a1.y - b1.y) - (a2.y - a1.y)*(a1.x - b1.x);
float det = (b2.y - b1.y)*(a2.x - a1.x) - (b2.x - b1.x)*(a2.y - a1.y);
if (std::abs(det) < 1e-4f)
return false;
det = 1.f / det;
out.x = u * det;
out.y = v * det;
return true;
}
bool IntersectLineSegmentLineSegment2D(const Vector2 &a1, const Vector2 &a2, const Vector2 &b1, const Vector2 &b2, Vector2 &out)
{
bool ret = IntersectLineLine2D(a1, a2, b1, b2, out);
return ret && out.x >= 0.f && out.x <= 1.f && out.y >= 0.f && out.y <= 1.f;
}
/// Returns true if poly[i+1] is an ear.
/// Precondition: i+2 == j (mod poly.size()).
bool IsAnEar(const std::vector<Vector2> &poly, int i, int j)
{
Vector2 dummy;
int x = (int)poly.size()-1;
for(int y = 0; y < i; ++y)
{
if (IntersectLineSegmentLineSegment2D(poly[i], poly[j], poly[x], poly[y], dummy))
return false;
x = y;
}
x = j+1;
for(int y = x+1; y < (int)poly.size(); ++y)
{
if (IntersectLineSegmentLineSegment2D(poly[i], poly[j], poly[x], poly[y], dummy))
return false;
x = y;
}
return true;
}
bool Polygon::Contains(const Polygon &worldSpacePolygon, float polygonThickness) const
{
for(int i = 0; i < worldSpacePolygon.NumVertices(); ++i)
if (!Contains(worldSpacePolygon.Vertex(i), polygonThickness))
return false;
return true;
}
bool Polygon::Contains(const Vector3 &worldSpacePoint, float polygonThicknessSq) const
{
// Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
if (p.size() < 3)
return false;
vec basisU = BasisU();
vec basisV = BasisV();
assert1(basisU.IsNormalized(), basisU);
assert1(basisV.IsNormalized(), basisV);
assert2(basisU.IsPerpendicular(basisV), basisU, basisV);
assert3(basisU.IsPerpendicular(PlaneCCW().normal), basisU, PlaneCCW().normal, basisU.Dot(PlaneCCW().normal));
assert3(basisV.IsPerpendicular(PlaneCCW().normal), basisV, PlaneCCW().normal, basisV.Dot(PlaneCCW().normal));
vec normal = basisU.Cross(basisV);
// float lenSq = normal.LengthSq(); ///\todo Could we treat basisU and basisV unnormalized here?
float dot = normal.Dot(vec(p[0]) - worldSpacePoint);
if (dot*dot > polygonThicknessSq)
return false; // The point is not even within the plane of the polygon - can't be contained.
int numIntersections = 0;
const float epsilon = 1e-4f;
// General strategy: transform all points on the polygon onto 2D face plane of the polygon, where the target query point is
// centered to lie in the origin.
// If the test ray (0,0) -> (+inf, 0) intersects exactly an odd number of polygon edge segments, then the query point must have been
// inside the polygon. The test ray is chosen like that to avoid all extra per-edge computations.
// This method works for both simple and non-simple (self-intersecting) polygons.
vec vt = vec(p.back()) - worldSpacePoint;
float2 p0 = float2(Dot(vt, basisU), Dot(vt, basisV));
if (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(int i = 0; i < (int)p.size(); ++i)
{
vt = vec(p[i]) - worldSpacePoint;
float2 p1 = float2(Dot(vt, basisU), Dot(vt, basisV));
if (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) // If the line segment p0 -> p1 straddles the line x=0, it could intersect the ray (0,0) -> (+inf, 0)
{
if (Min(p0.x, p1.x) > 0.f) // If both x-coordinates are positive, then there certainly is an intersection with the ray.
++numIntersections;
else if (Max(p0.x, p1.x) > 0.f) // If one of them is positive, there could be an intersection. (otherwise both are negative and they can't intersect ray)
{
// 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?
float2 d = p1 - p0;
if (d.y != 0.f)
{
float t = -p0.y / d.y; // The line segment parameter, t \in [0,1] forms the line segment p0->p1.
float x = p0.x + t * d.x; // The x-coordinate of intersection with the ray.
if (t >= 0.f && t <= 1.f && x > 0.f)
++numIntersections;
}
}
}
p0 = p1;
}
return numIntersections % 2 == 1;
}
bool Polygon::Contains(const Vector3 &worldSpacePoint, float polygonThicknessSq) const
{
// Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
if (vertices.size() < 3)
return false;
Vector3 basisU = BasisU();
Vector3 basisV = BasisV();
assert(basisU.IsNormalized());
assert(basisV.IsNormalized());
assert(basisU.IsPerpendicular(basisV));
assert(basisU.IsPerpendicular(PlaneCCW().Normal));
assert(basisV.IsPerpendicular(PlaneCCW().Normal));
Vector3 normal = basisU.Cross(basisV);
// float lenSq = normal.LengthSq(); ///\todo Could we treat basisU and basisV unnormalized here?
float dot = normal.Dot(Vector3(vertices[0]) - worldSpacePoint);
if (dot*dot > polygonThicknessSq)
return false; // The point is not even within the plane of the polygon - can't be contained.
int numIntersections = 0;
const float epsilon = 1e-4f;
// General strategy: transform all points on the polygon onto 2D face plane of the polygon, where the target query point is
// centered to lie in the origin.
// If the test ray (0,0) -> (+inf, 0) intersects exactly an odd number of polygon edge segments, then the query point must have been
// inside the polygon. The test ray is chosen like that to avoid all extra per-edge computations.
// This method works for both simple and non-simple (self-intersecting) polygons.
Vector3 vt = Vector3(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(int i = 0; i < (int)vertices.size(); ++i)
{
vt = Vector3(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) // If the line segment p0 -> p1 straddles the line x=0, it could intersect the ray (0,0) -> (+inf, 0)
{
if (std::min(p0.x, p1.x) > 0.f) // If both x-coordinates are positive, then there certainly is an intersection with the ray.
++numIntersections;
else if (std::max(p0.x, p1.x) > 0.f) // If one of them is positive, there could be an intersection. (otherwise both are negative and they can't intersect ray)
{
// 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; // The line segment parameter, t \in [0,1] forms the line segment p0->p1.
float x = p0.x + t * d.x; // The x-coordinate of intersection with the ray.
if (t >= 0.f && t <= 1.f && x > 0.f)
++numIntersections;
}
}
}
p0 = p1;
}
return numIntersections % 2 == 1;
}
bool Polygon::Contains(const LineSegment &worldSpaceLineSegment, float polygonThickness) const
{
if (vertices.size() < 3)
return false;
Plane plane = PlaneCCW();
if (plane.Distance(worldSpaceLineSegment.A) > polygonThickness ||
plane.Distance(worldSpaceLineSegment.B) > polygonThickness)
return false;
// For robustness, project onto the polygon plane.
LineSegment l = plane.Project(worldSpaceLineSegment);
if (!Contains(l.A) || !Contains(l.B))
return false;
for(int i = 0; i < (int)vertices.size(); ++i)
if (plane.Project(Edge(i)).Intersects(l))
return false;
return true;
}
bool Polygon::Contains(const Triangle &worldSpaceTriangle, float polygonThickness) const
{
return Contains(worldSpaceTriangle.Edge(0), polygonThickness) &&
Contains(worldSpaceTriangle.Edge(1), polygonThickness) &&
Contains(worldSpaceTriangle.Edge(2), polygonThickness);
}
/** The implementation of this function is based on the paper
"Kong, Everett, Toussant. The Graham Scan Triangulates Simple Polygons."
See also p. 772-775 of Geometric Tools for Computer Graphics.
The running time of this function is O(n^2). */
std::vector<Triangle> Polygon::Triangulate() const
{
assert(IsPlanar());
// assume1(IsPlanar(), this->SerializeToString()); // TODO: enable
std::vector<Triangle> t;
// Handle degenerate cases.
if (NumVertices() < 3)
return t;
if (NumVertices() == 3)
{
t.push_back(Triangle(Vertex(0), Vertex(1), Vertex(2)));
return t;
}
std::vector<Vector2> p2d;
std::vector<int> polyIndices;
for(int v = 0; v < NumVertices(); ++v)
{
p2d.push_back(MapTo2D(v));
polyIndices.push_back(v);
}
// Clip ears of the polygon until it has been reduced to a triangle.
int i = 0;
int j = 1;
int k = 2;
size_t numTries = 0; // Avoid creating an infinite loop.
while(p2d.size() > 3 && numTries < p2d.size())
{
if (Vector2::OrientedCCW(p2d[i], p2d[j], p2d[k]) && IsAnEar(p2d, i, k))
{
// The vertex j is an ear. Clip it off.
t.push_back(Triangle(vertices[polyIndices[i]], vertices[polyIndices[j]], vertices[polyIndices[k]]));
p2d.erase(p2d.begin() + j);
polyIndices.erase(polyIndices.begin() + j);
// The previous index might now have become an ear. Move back one index to see if so.
if (i > 0)
{
i = (i + (int)p2d.size() - 1) % p2d.size();
j = (j + (int)p2d.size() - 1) % p2d.size();
k = (k + (int)p2d.size() - 1) % p2d.size();
}
numTries = 0;
}
else
{
// The vertex at j is not an ear. Move to test next vertex.
i = j;
j = k;
k = (k+1) % p2d.size();
++numTries;
}
}
assert(p2d.size() == 3);
if (p2d.size() > 3) // If this occurs, then the polygon is NOT counter-clockwise oriented.
return t;
/*
{
// For conveniency, create a copy that has the winding order fixed, and triangulate that instead.
// (Causes a large performance hit!)
Polygon p2 = *this;
for(size_t i = 0; i < p2.p.size()/2; ++i)
std::swap(p2.p[i], p2.p[p2.p.size()-1-i]);
return p2.Triangulate();
}
*/
// Add the last poly.
t.push_back(Triangle(vertices[polyIndices[0]], vertices[polyIndices[1]], vertices[polyIndices[2]]));
return t;
}
bool Polygon::Intersects(const Capsule &capsule) const {
///@todo Optimize.
std::vector<Triangle> tris = Triangulate();
for(size_t i = 0; i < tris.size(); ++i)
if (tris[i].Intersects(capsule))
return true;
return false;
}
bool Polygon::Intersects(const Line &line) const {
float d;
if (!PlaneCCW().Intersects(line, &d))
return false;
return Contains(line.GetPoint(d));
}
bool Polygon::Intersects(const Ray &ray) const
{
float d;
if (!PlaneCCW().Intersects(ray, &d))
return false;
return Contains(ray.GetPoint(d));
}
bool Polygon::Intersects(const LineSegment &lineSegment) const
{
Plane plane = PlaneCCW();
// Compute line-plane intersection (unroll Plane::IntersectLinePlane())
float denom = Dot(plane.normal, lineSegment.b - lineSegment.a);
if (Abs(denom) < 1e-4f) // The plane of the polygon and the line are planar? Do the test in 2D.
return Intersects2D(LineSegment(POINT_VEC(MapTo2D(lineSegment.a), 0), POINT_VEC(MapTo2D(lineSegment.b), 0)));
// The line segment properly intersects the plane of the polygon, so there is exactly one
// point of intersection between the plane of the polygon and the line segment. Test that intersection point against
// the line segment end points.
float t = (plane.d - Dot(plane.normal, lineSegment.a)) / denom;
if (t < 0.f || t > 1.f)
return false;
return Contains(lineSegment.GetPoint(t));
}
bool Polygon::Intersects(const Plane &plane) const
{
// Project the points of this polygon onto the 1D axis of the plane normal.
// If there are points on both sides of the plane, then the polygon intersects the plane.
float minD = inf;
float maxD = -inf;
for(size_t i = 0; i < p.size(); ++i)
{
float d = plane.SignedDistance(p[i]);
minD = Min(minD, d);
maxD = Max(maxD, d);
}
// Allow a very small epsilon tolerance.
return minD <= 1e-4f && maxD >= -1e-4f;
}
bool Polygon::ConvexIntersects(const AABB &aabb) const
{
return GJKIntersect(*this, aabb);
}
bool Polygon::ConvexIntersects(const OBB &obb) const
{
return GJKIntersect(*this, obb);
}
bool Polygon::ConvexIntersects(const Frustum &frustum) const
{
return GJKIntersect(*this, frustum);
}
template<typename Convex /* = AABB, OBB, Frustum. */>
bool Convex_Intersects_Polygon(const Convex &c, const Polygon &p)
{
LineSegment l;
l.a = p.p.back();
for(size_t i = 0; i < p.p.size(); ++i)
{
l.b = p.p[i];
if (c.Intersects(l))
return true;
l.a = l.b;
}
// Check all the edges of the convex shape against the polygon.
for(int i = 0; i < c.NumEdges(); ++i)
{
l = c.Edge(i);
if (p.Intersects(l))
return true;
}
return false;
}
bool Polygon::Intersects(const AABB &aabb) const
{
// Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the aabb)
bool convexIntersects = ConvexIntersects(aabb);
if (!convexIntersects)
return false;
return Convex_Intersects_Polygon(aabb, *this);
}
bool Polygon::Intersects(const OBB &obb) const
{
// Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the obb)
bool convexIntersects = ConvexIntersects(obb);
if (!convexIntersects)
return false;
return Convex_Intersects_Polygon(obb, *this);
}
bool Polygon::Intersects(const Frustum &frustum) const
{
// Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the frustum)
bool convexIntersects = ConvexIntersects(frustum);
if (!convexIntersects)
return false;
return Convex_Intersects_Polygon(frustum, *this);
}
}

241
src/J3ML/Geometry/Polyhedron.cpp
View File

@@ -3,11 +3,42 @@
#include <J3ML/Geometry/Triangle.h>
#include <J3ML/Geometry/LineSegment.h>
#include "J3ML/Geometry/Ray.h"
#include "J3ML/Geometry/Line.h"
#include <J3ML/Geometry/Polygon.h>
#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
{
@@ -160,8 +191,89 @@ namespace J3ML::Geometry
return (bestNumIntersections % 2) == 1;
}
float Polyhedron::FaceContainmentDistance2D(int i, Vector3 vector3) const {
return 0;
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).Normalize();
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
@@ -231,7 +343,7 @@ namespace J3ML::Geometry
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));
return Plane(v[face.v[0]], Vector3(0,1,0));
else
return Plane();
}
@@ -339,17 +451,17 @@ namespace J3ML::Geometry
#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());
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 cv(0, 1, 0, 0);
return Vector4(0, 1, 0, 0);
else
return float4::nan;
return Vector4::NaN;
}
vec Polyhedron::FaceNormal(int faceIndex) const
Vector3 Polyhedron::FaceNormal(int faceIndex) const
{
cv normal = PolyFaceNormal(*this, faceIndex);
return DIR_VEC((float)normal.x, (float)normal.y, (float)normal.z);
Vector4 normal = PolyFaceNormal(*this, faceIndex);
return Vector3((float)normal.x, (float)normal.y, (float)normal.z);
}
bool Polyhedron::IsConvex() const
@@ -413,6 +525,117 @@ namespace J3ML::Geometry
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;
}
}

71
src/J3ML/Geometry/Ray.cpp
View File

@@ -4,6 +4,19 @@
namespace J3ML::Geometry
{
void Ray::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
{
outMin = outMax = Vector3::Dot(direction, Origin);
float d = Vector3::Dot(direction, Direction);
// Most of the time, the projection interval will be a half-infinite range, extending to either -inf or +inf.
if (d > 1e-4f)
outMax = INFINITY;
else if (d < -1e4f)
outMin = -INFINITY;
}
RaycastResult Ray::Cast(const Sphere &target, float maxDistance)
{
Vector3 p0 = this->Origin;
@@ -121,6 +134,64 @@ namespace J3ML::Geometry
return RaycastResult::NoHit();
}
Vector3 Ray::ClosestPoint(const Vector3 &targetPoint, float &d) const
{
d = std::max(0.f, Vector3::Dot(targetPoint - Origin, Direction));
return GetPoint(d);
}
Vector3 Ray::ClosestPoint(const LineSegment &other, float &d, float &d2) const {
Line::ClosestPointLineLine(Origin, Direction, other.A, other.B - other.A, d, d2);
if (d < 0.f)
{
d = 0.f;
if (d2 >= 0.f && d2 <= 1.f)
{
other.ClosestPoint(Origin, d2);
return Origin;
}
Vector3 p;
float t2;
if (d2 < 0.f)
{
p = other.A;
t2 = 0.f;
}
else // u2 > 1.f
{
p = other.B;
t2 = 1.f;
}
Vector3 closestPoint = ClosestPoint(p, d);
Vector3 closestPoint2 = other.ClosestPoint(Origin, d2);
if (closestPoint.DistanceSquared(p) <= closestPoint2.DistanceSquared(Origin))
{
d2 = t2;
return closestPoint;
}
else
{
d = 0.f;
return Origin;
}
}
else if (d2 < 0.f)
{
d2 = 0.f;
return ClosestPoint(other.A, d);
}
else if (d2 > 1.f)
{
d2 = 1.f;
return ClosestPoint(other.B, d);
}
else
return GetPoint(d);
}
}

10
src/J3ML/Geometry/Sphere.cpp
View File

@@ -3,4 +3,14 @@
namespace J3ML::Geometry
{
bool Sphere::Contains(const LineSegment &lineseg) const {
}
void Sphere::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
{
float d = Vector3::Dot(direction, Position);
outMin = d - Radius;
outMax = d + Radius;
}
}

336
src/J3ML/Geometry/Triangle.cpp
View File

@@ -1,9 +1,63 @@
#include <J3ML/Geometry/Triangle.h>
#include <J3ML/LinearAlgebra.h>
#include <J3ML/Geometry/AABB.h>
#include <J3ML/Geometry/LineSegment.h>
#include <J3ML/Geometry/Line.h>
#include <J3ML/Geometry/Capsule.h>
namespace J3ML::Geometry
{
LineSegment Triangle::Edge(int i) const
{
assert(0 <= i);
assert(i <= 2);
if (i == 0)
return LineSegment(V0, V1);
else if (i == 1)
return LineSegment(V1, V2);
else if (i == 2)
return LineSegment(V2, V0);
else
return LineSegment(Vector3::NaN, Vector3::NaN);
}
Vector3 Triangle::Vertex(int i) const
{
assert(0 <= i);
assert(i <= 2);
if (i == 0)
return V0;
else if (i == 1)
return V1;
else if (i == 2)
return V2;
else
return Vector3::NaN;
}
Plane Triangle::PlaneCCW() const
{
return Plane(V0, V1, V2);
}
Plane Triangle::PlaneCW() const
{
return Plane(V0, V1, V2);
}
void Triangle::ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const
{
dMin = dMax = Vector3::Dot(axis, V0);
float t = Vector3::Dot(axis, V1);
dMin = std::min(t, dMin);
dMax = std::max(t, dMax);
t = Vector3::Dot(axis, V2);
dMin = std::min(t, dMin);
dMax = std::max(t, dMax);
}
AABB Triangle::BoundingAABB() const {
AABB aabb;
aabb.minPoint = Vector3::Min(V0, V1, V2);
@@ -14,4 +68,286 @@ namespace J3ML::Geometry
bool Triangle::Intersects(const AABB &aabb) const {
return aabb.Intersects(*this);
}
/** Calculates the intersection between a line and a triangle. The facing is not accounted for, so
rays are reported to intersect triangles that are both front and backfacing.
According to "T. M&ouml;ller, B. Trumbore. Fast, Minimum Storage Ray/Triangle Intersection. 2005."
http://jgt.akpeters.com/papers/MollerTrumbore97/
@param linePos The starting point of the line.
@param lineDir The direction vector of the line. This does not need to be normalized.
@param v0 Vertex 0 of the triangle.
@param v1 Vertex 1 of the triangle.
@param v2 Vertex 2 of the triangle.
@param u [out] The barycentric u coordinate is returned here if an intersection occurred.
@param v [out] The barycentric v coordinate is returned here if an intersection occurred.
@return The distance along the ray to the point of intersection, or +inf if no intersection occurred.
If no intersection, then u and v and t will contain undefined values. If lineDir was not normalized, then to get the
real world-space distance, one must scale the returned value with lineDir.Length(). If the returned value is negative,
then the intersection occurs 'behind' the line starting position, with respect to the direction vector lineDir. */
float Triangle::IntersectLineTri(const Vector3 &linePos, const Vector3 &lineDir,
const Vector3 &v0, const Vector3 &v1, const Vector3 &v2,
float &u, float &v)
{
const float epsilon = 1e-4f;
// Edge vectors
Vector3 vE1 = v1 - v0;
Vector3 vE2 = v2 - v0;
// begin calculating determinant - also used to calculate U parameter
Vector3 vP = lineDir.Cross(vE2);
// If det < 0, intersecting backfacing tri, > 0, intersecting frontfacing tri, 0, parallel to plane.
const float det = vE1.Dot(vP);
// If determinant is near zero, ray lies in plane of triangle.
if (std::abs(det) <= epsilon)
return INFINITY;
const float recipDet = 1.f / det;
// Calculate distance from v0 to ray origin
Vector3 vT = linePos - v0;
// Output barycentric u
u = vT.Dot(vP) * recipDet;
if (u < -epsilon || u > 1.f + epsilon)
return INFINITY; // Barycentric U is outside the triangle - early out.
// Prepare to test V parameter
Vector3 vQ = vT.Cross(vE1);
// Output barycentric v
v = lineDir.Dot(vQ) * recipDet;
if (v < -epsilon || u + v > 1.f + epsilon) // Barycentric V or the combination of U and V are outside the triangle - no intersection.
return INFINITY;
// Barycentric u and v are in limits, the ray intersects the triangle.
// Output signed distance from ray to triangle.
return vE2.Dot(vQ) * recipDet;
// return (det < 0.f) ? IntersectBackface : IntersectFrontface;
}
Vector3 Triangle::ClosestPointToTriangleEdge(const Line &other, float *outU, float *outV, float *outD) const
{
///@todo Optimize!
// The line is parallel to the triangle.
float unused1, unused2, unused3;
float d1, d2, d3;
Vector3 pt1 = Edge(0).ClosestPoint(other, unused1, d1);
Vector3 pt2 = Edge(1).ClosestPoint(other, unused2, d2);
Vector3 pt3 = Edge(2).ClosestPoint(other, unused3, d3);
float dist1 = pt1.DistanceSq(other.GetPoint(d1));
float dist2 = pt2.DistanceSq(other.GetPoint(d2));
float dist3 = pt3.DistanceSq(other.GetPoint(d3));
if (dist1 <= dist2 && dist1 <= dist3)
{
if (outU) *outU = BarycentricUV(pt1).x;
if (outV) *outV = BarycentricUV(pt1).y;
if (outD) *outD = d1;
return pt1;
}
else if (dist2 <= dist3)
{
if (outU) *outU = BarycentricUV(pt2).x;
if (outV) *outV = BarycentricUV(pt2).y;
if (outD) *outD = d2;
return pt2;
}
else
{
if (outU) *outU = BarycentricUV(pt3).x;
if (outV) *outV = BarycentricUV(pt3).y;
if (outD) *outD = d3;
return pt3;
}
}
Vector3 Triangle::ClosestPointToTriangleEdge(const LineSegment &lineSegment, float *outU, float *outV, float *outD) const
{
///@todo Optimize!
// The line is parallel to the triangle.
float unused1, unused2, unused3;
float d1, d2, d3;
Vector3 pt1 = Edge(0).ClosestPoint(lineSegment, unused1, d1);
Vector3 pt2 = Edge(1).ClosestPoint(lineSegment, unused2, d2);
Vector3 pt3 = Edge(2).ClosestPoint(lineSegment, unused3, d3);
float dist1 = pt1.DistanceSq(lineSegment.GetPoint(d1));
float dist2 = pt2.DistanceSq(lineSegment.GetPoint(d2));
float dist3 = pt3.DistanceSq(lineSegment.GetPoint(d3));
if (dist1 <= dist2 && dist1 <= dist3)
{
if (outU) *outU = BarycentricUV(pt1).x;
if (outV) *outV = BarycentricUV(pt1).y;
if (outD) *outD = d1;
return pt1;
}
else if (dist2 <= dist3)
{
if (outU) *outU = BarycentricUV(pt2).x;
if (outV) *outV = BarycentricUV(pt2).y;
if (outD) *outD = d2;
return pt2;
}
else
{
if (outU) *outU = BarycentricUV(pt3).x;
if (outV) *outV = BarycentricUV(pt3).y;
if (outD) *outD = d3;
return pt3;
}
}
Vector3 Triangle::ClosestPoint(const Vector3 &p) const
{
/** The code for Triangle-float3 test is from Christer Ericson's Real-Time Collision Detection, pp. 141-142. */
// Check if P is in vertex region outside A.
Vector3 ab = V1 - V0;
Vector3 ac = V2 - V0;
Vector3 ap = p - V0;
float d1 = Vector3::Dot(ab, ap);
float d2 = Vector3::Dot(ac, ap);
if (d1 <= 0.f && d2 <= 0.f)
return V0; // Barycentric coordinates are (1,0,0).
// Check if P is in vertex region outside B.
Vector3 bp = p - V1;
float d3 = Vector3::Dot(ab, bp);
float d4 = Vector3::Dot(ac, bp);
if (d3 >= 0.f && d4 <= d3)
return V1; // Barycentric coordinates are (0,1,0).
// Check if P is in edge region of AB, and if so, return the projection of P onto AB.
float vc = d1*d4 - d3*d2;
if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f)
{
float v = d1 / (d1 - d3);
return V0 + v * ab; // The barycentric coordinates are (1-v, v, 0).
}
// Check if P is in vertex region outside C.
Vector3 cp = p - V2;
float d5 = Vector3::Dot(ab, cp);
float d6 = Vector3::Dot(ac, cp);
if (d6 >= 0.f && d5 <= d6)
return V2; // The barycentric coordinates are (0,0,1).
// Check if P is in edge region of AC, and if so, return the projection of P onto AC.
float vb = d5*d2 - d1*d6;
if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f)
{
float w = d2 / (d2 - d6);
return V0 + w * ac; // The barycentric coordinates are (1-w, 0, w).
}
// Check if P is in edge region of BC, and if so, return the projection of P onto BC.
float va = d3*d6 - d5*d4;
if (va <= 0.f && d4 - d3 >= 0.f && d5 - d6 >= 0.f)
{
float w = (d4 - d3) / (d4 - d3 + d5 - d6);
return V1 + w * (V2 - V0); // The barycentric coordinates are (0, 1-w, w).
}
// P must be inside the face region. Compute the closest point through its barycentric coordinates (u,v,w).
float denom = 1.f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
return V0 + ab * v + ac * w;
}
Vector3 Triangle::ClosestPoint(const LineSegment& lineSegment, Vector3 *otherPt) const
{
///\todo Optimize.
float u, v;
float t = IntersectLineTri(lineSegment.A, lineSegment.B - lineSegment.A, V0, V1, V2, u, v);
bool intersects = (t >= 0.0f && t <= 1.0f);
if (intersects)
{
// assume3(lineSegment.GetPoint(t).Equals(this->Point(u, v)), lineSegment.GetPoint(t).SerializeToCodeString(), this->Point(u, v).SerializeToCodeString(), lineSegment.GetPoint(t).Distance(this->Point(u, v)));
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
return this->Point(u, v);
}
float u1,v1,d1;
Vector3 pt1 = ClosestPointToTriangleEdge(lineSegment, &u1, &v1, &d1);
Vector3 pt2 = ClosestPoint(lineSegment.A);
Vector3 pt3 = ClosestPoint(lineSegment.B);
float D1 = pt1.DistanceSq(lineSegment.GetPoint(d1));
float D2 = pt2.DistanceSq(lineSegment.A);
float D3 = pt3.DistanceSq(lineSegment.B);
if (D1 <= D2 && D1 <= D3)
{
if (otherPt)
*otherPt = lineSegment.GetPoint(d1);
return pt1;
}
else if (D2 <= D3)
{
if (otherPt)
*otherPt = lineSegment.A;
return pt2;
}
else
{
if (otherPt)
*otherPt = lineSegment.B;
return pt3;
}
}
/// Implementation from Christer Ericson's Real-Time Collision Detection, pp. 51-52.
inline float TriArea2D(float x1, float y1, float x2, float y2, float x3, float y3)
{
return (x1-x2)*(y2-y3) - (x2-x3)*(y1-y2);
}
Vector3 Triangle::BarycentricUVW(const Vector3 &point) const {
// Implementation from Christer Ericson's Real-Time Collision Detection, pp. 51-52.
// Unnormalized triangle normal.
Vector3 m = Vector3::Cross(V1-V0, V2-V1);
// Nominators and one-over-denominator for u and v ratios.
float nu, nv, ood;
// Absolute components for determining projection plane.
float x = std::abs(m.x);
float y = std::abs(m.y);
float z = std::abs(m.z);
if (x >= y && x >= z)
{
// Project to the yz plane.
nu = TriArea2D(point.y, point.z, V1.y, V1.z, V2.y, V2.z); // Area of PBC in yz-plane.
nv = TriArea2D(point.y, point.z, V2.y, V2.z, V0.y, V0.z); // Area OF PCA in yz-plane.
ood = 1.f / m.x; // 1 / (2*area of ABC in yz plane)
}
else if (y >= z) // Note: The book has a redundant 'if (y >= x)' comparison
{
// y is largest, project to the xz-plane.
nu = TriArea2D(point.x, point.z, V1.x, V1.z, V2.x, V2.z);
nv = TriArea2D(point.x, point.z, V2.x, V2.z, V0.x, V0.z);
ood = 1.f / -m.y;
}
else // z is largest, project to the xy-plane.
{
nu = TriArea2D(point.x, point.y, V1.x, V1.y, V2.x, V2.y);
nv = TriArea2D(point.x, point.y, V2.x, V2.y, V0.x, V0.y);
ood = 1.f / m.z;
}
float u = nu * ood;
float v = nv * ood;
float w = 1.f - u - v;
return Vector3(u,v,w);
}
bool Triangle::Intersects(const Capsule &capsule) const {
return capsule.Intersects(*this);
}
}

11
src/J3ML/LinearAlgebra/Vector2.cpp
View File

@@ -333,4 +333,15 @@ namespace J3ML::LinearAlgebra {
float Vector2::DistanceSq(const Vector2 &from, const Vector2 &to) {
return (from-to).LengthSquared();
}
bool Vector2::OrientedCCW(const Vector2 &a, const Vector2 &b, const Vector2 &c)
{
// Compute the determinant
// | ax ay 1 |
// | bx by 1 |
// | cx cy 1 |
// See Christer Ericson, Real-Time Collision Detection, p.32.
return (a.x-c.x)*(b.y-c.y) - (a.y-c.y)*(b.x-c.x) >= 0.f;
}
}

6
src/J3ML/LinearAlgebra/Vector3.cpp
View File

@@ -475,5 +475,11 @@ namespace J3ML::LinearAlgebra {
return ptr()[index];
}
void Vector3::Set(float d, float d1, float d2) {
x = d;
y = d1;
z = d2;
}
}