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

Giga Geometry Implementation

Browse Source
This commit is contained in:
josh
2024-04-08 13:27:56 -04:00
parent 0aff68b63e
commit d7b2157b0c
32 changed files with 1235 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
include/J3ML/Algorithm/GJK.h Normal file
View File

@@ -0,0 +1,60 @@
// @file GJK.h
/// Implementation of the Gilbert-Johnson-Keerthi (GJK) convex polyhedron intersection test
#include <J3ML/LinearAlgebra.h>
#include <J3ML/Geometry.h>
#pragma once
namespace J3ML::Algorithms
{
Vector3 UpdateSimplex(Vector3 *s, int &n);
#define SUPPORT(dir, minS, maxS) (a.ExtremePoint(dir, maxS) - b.ExtremePoint(-dir, minS));
template <typename A, typename B>
bool GJKIntersect(const A &a, const B &b)
{
Vector3 support[4];
// Start with an arbitrary point in the Minkowski set shape.
support[0] = a.AnyPointFast() - b.AnyPointFast();
if (support[0].LengthSquared() < 1e-7f) // Robustness check: Test if the first arbitrary point we guessed produced the zero vector we are looking for!
return true;
Vector3 d = -support[0]; // First search direction is straight toward the origin from the found point.
int n = 1; // Stores the current number of points in the search simplex.
int nIterations = 50; // Robustness check: Limit the maximum number of iterations to perform to avoid infinite loop if types A or B are buggy!
while (nIterations-- > 0)
{
// Compute the extreme point to the direction d in the Minkowski set shape.
float maxS, minS;
Vector3 newSupport = SUPPORT(d, minS, maxS);
// If the most extreme point in that search direction did not walk past the origin, then the origin cannot be contained in the Minkowski
// convex shape, and the two convex objects a and b do not share a common point - no intersection!
if (minS + maxS < 0.f)
return false;
// Add the newly evaluated point to the search simplex
assert(n < 4);
support[n++] = newSupport;
// Examine the current simplex, prune a redundant part of it, and produce the next search direction.
d = UpdateSimplex(support, n);
if (n == 0) // Was the origin contained in the current simplex? If so, then the convex shapes a and b do share a common point - intersection!
return true;
}
return false;
}
// This computes GJL intersection, but by first translating both objects to a coordinate frame that is as closely
// centered around world origin as possible, to gain floating point precision.
template <typename A, typename B>
bool FloatingPointOffsetedGJKIntersect(const A &a, const B &b)
{
AABB ab = a.MinimalEnclosingAABB();
AABB bb = b.MinimalEnclosingAABB();
Vector3 offset = (Vector3::Min(ab.minPoint, bb.minPoint) + Vector3::Max(ab.maxPoint, bb.maxPoint)) * 0.5f;
const Vector3 floatingPtPrecisionOffset = -offset;
return GJLIntersect(a.Translated(floatingPtPrecisionOffset), b.Translated(floatingPtPrecisionOffset));
}
}

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

@@ -25,31 +25,23 @@ namespace J3ML::Geometry
public:
Vector3 minPoint;
Vector3 maxPoint;
public:
static int NumFaces() { return 6; }
static int NumEdges() { return 12; }
static int NumVertices() { return 8; }
public:
AABB();
AABB(const Vector3& min, const Vector3& max);
Vector3 HalfDiagonal() const { return HalfSize(); }
static int NumFaces() { return 6; }
static int NumEdges() { return 12; }
static int NumVertices() { return 8; }
static AABB FromCenterAndSize(const Vector3 &center, const Vector3 &size);
float MinX() const;
float MinY() const;
float MinZ() const;
float MaxX() const;
float MaxY() const;
float MaxZ() const;
/// Returns the smallest sphere that contains this AABB.
@@ -79,8 +71,7 @@ namespace J3ML::Geometry
Vector3 CornerPoint(int cornerIndex) const;
Vector3 ExtremePoint(const Vector3 &direction) const;
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance);
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
Vector3 PointOnEdge(int edgeIndex, float u) const;
@@ -126,8 +117,11 @@ namespace J3ML::Geometry
bool Intersects(const Frustum& frustum) const;
bool Intersects(const Polyhedron& polyhedron) const;
TriangleMesh Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const;
/// Finds the set intersection of this and the given AABB.
/** @return This function returns the AABB that is contained in both this and the given AABB.
@todo Add Intersection(OBB/Polyhedron). */
AABB Intersection(const AABB& rhs) const;
bool IntersectLineAABB(const Vector3& linePos, const Vector3& lineDir, float tNear, float tFar) const;
void SetFrom(const Vector3 *pVector3, int i);
@@ -154,6 +148,27 @@ namespace J3ML::Geometry
void Enclose(const OBB &obb);
bool TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const;
bool Intersects(const LineSegment &lineSegment) const;
/// Computes the intersection of a line, ray or line segment and an AABB.
/** Based on "T. Kay, J. Kajiya. Ray Tracing Complex Scenes. SIGGRAPH 1986 vol 20, number 4. pp. 269-"
http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm
@param linePos The starting position of the line.
@param lineDir The direction of the line. This direction vector must be normalized!
@param tNear [in, out] For the test, the input line is treated as a line segment. Pass in the signed distance
from the line origin to the start of the line. For a Line-AABB test, -FLOAT_INF is typically passed here.
For a Ray-AABB test, 0.0f should be inputted. If intersection occurs, the signed distance from line origin
to the line entry point in the AABB is returned here.
@param tFar [in, out] Pass in the signed distance from the line origin to the end of the line. For Line-AABB and
Ray-AABB tests, pass in FLOAT_INF. For a LineSegment-AABB test, pass in the length of the line segment here.
If intersection occurs, the signed distance from line origin to the line exit point in the AABB
is returned here.
@return True if an intersection occurs, false otherwise.
@note This is a low level utility function. It may be more convenient to use one of the AABB::Intersects()
functions instead.
@see Intersects(). */
bool IntersectLineAABB(const Vector3& linePos, const Vector3& lineDir, float tNear, float tFar) const;
bool IntersectLineAABB_CPP(const Vector3 &linePos, const Vector3 &lineDir, float &tNear, float &tFar) const;
};

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

@@ -24,10 +24,23 @@ namespace J3ML::Geometry
LineSegment l;
// Specifies the radius of this capsule
float r;
public:
Capsule();
Capsule(const LineSegment& endPoints, float radius);
Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
/// Quickly returns an arbitrary point inside this Capsule. Used in GJK intersection test.
inline Vector3 AnyPointFast() const { return l.A; }
/// Computes the extreme point of this Capsule in the given direction.
/** An extreme point is a farthest point of this Capsule in the given direction. Given a direction,
this point is not necessarily unique.
@param direction The direction vector of the direction to find the extreme point. This vector may
be unnormalized, but may not be null.
@return The extreme point of this Capsule in the given direction. */
Vector3 ExtremePoint(const Vector3 &direction) const;
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
bool IsDegenerate() const;
float Height() const;
float Diameter() const;

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

@@ -139,7 +139,23 @@ namespace J3ML::Geometry
Matrix4x4 projectionMatrix;
Matrix4x4 viewProjectionMatrix;
public: /// Methods
Frustum();
Frustum()
: type(FrustumType::Invalid),
pos(Vector3::NaN),
front(Vector3::NaN),
up(Vector3::NaN),
nearPlaneDistance(NAN),
farPlaneDistance(NAN),
worldMatrix(Matrix4x4::NaN),
viewProjectionMatrix(Matrix4x4::NaN)
{
// For conveniency, allow automatic initialization of the graphics API and handedness in use.
// If neither of the #defines are set, user must specify per-instance.
}
/// Quickly returns an arbitrary point inside this Frustum. Used in GJK intersection test.
inline Vector3 AnyPointFast() const { return CornerPoint(0); }
static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
AABB MinimalEnclosingAABB() const;
@@ -175,7 +191,13 @@ namespace J3ML::Geometry
Vector3 NearPlanePos(float x, float y) const;
Vector3 FarPlanePos(float x, float y) const;
Vector3 WorldRight() const;
Vector3 WorldRight() const
{
if (handedness == FrustumHandedness::Right)
return Vector3::Cross(front, up);
else
return Vector3::Cross(up, front);
}
Plane TopPlane() const;
Plane BottomPlane() const;
@@ -211,5 +233,11 @@ namespace J3ML::Geometry
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
void GetCornerPoints(Vector3 *outPointArray) const;
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
LineSegment Edge(int edgeIndex) const;
bool Intersects(const Line &line) const;
};
}

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

@@ -25,11 +25,35 @@ namespace J3ML::Geometry
class LineSegment
{
public:
LineSegment();
LineSegment(const Vector3& a, const Vector3& b);
Vector3 A;
Vector3 B;
bool Contains(const Vector3&) const;
public:
LineSegment();
LineSegment(const Vector3& a, const Vector3& b);
/// Computes the closest point on this line segment to the given object.
/** @param d [out] If specified, this parameter receives the normalized distance along
this line segment which specifies the closest point on this line segment to
the specified point.
@return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Vector3 &point) const;
bool Contains(const Vector3& point, float distanceThreshold = 1e-3f) const;
/// Quickly returns an arbitrary point inside this LineSegment. Used in GJK intersection test.
inline Vector3 AnyPointFast() const { return A; }
/// Computes an extreme point of this LineSegment in the given direction.
/** An extreme point is a farthest point along this LineSegment in the given direction. Given a direction,
this point is not necessarily unique.
@param direction The direction vector of the direction to find the extreme point. This vector may
be unnormalized, but may not be null.
@return An extreme point of this LineSegment in the given direction. The returned point is always
either a or b.
@see a, b.*/
Vector3 ExtremePoint(const Vector3 &direction) const;
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
Vector3 GetPoint(float d) const;
@@ -60,5 +84,9 @@ namespace J3ML::Geometry
Vector3 ClosestPoint(const LineSegment &other, float &d, float &d2) const;
Vector3 ClosestPoint(const Line &other, float &d, float &d2) const;
bool Intersects(const LineSegment &segment) const;
};
LineSegment operator *(const Matrix4x4 &transform, const LineSegment &l);
}

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

@@ -39,6 +39,8 @@ namespace J3ML::Geometry {
return aabb;
}
bool Intersects(const LineSegment &lineSegment) const;
Sphere MinimalEnclosingSphere() const;
Sphere MaximalContainedSphere() const;
Vector3 Size() const;
@@ -53,7 +55,7 @@ namespace J3ML::Geometry {
Vector3 CenterPoint() const;
Vector3 Centroid() const;
Vector3 AnyPointFast() const;
Vector3 AnyPointFast() const { return pos; }
float Volume() const;
float SurfaceArea() const;
@@ -96,5 +98,9 @@ namespace J3ML::Geometry {
void SetFrom(const AABB &aabb, const Quaternion &transform);
bool Intersects(const Plane& plane) const;
Matrix4x4 LocalToWorld() const;
Matrix4x4 WorldToLocal() const;
};
}

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

@@ -80,5 +80,7 @@ namespace J3ML::Geometry
bool Intersects(const Polyhedron &polyhedron) const;
LineSegment Project(const LineSegment &segment);
Vector3 Project(const Vector3 &point) const;
};
}

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

@@ -11,6 +11,10 @@ namespace J3ML::Geometry {
{
public:
std::vector<Vector3> vertices;
/// Quickly returns an arbitrary point inside this AABB. Used in GJK intersection test.
Vector3 AnyPointFast() const { return !vertices.empty() ? vertices[0] : Vector3::NaN; }
AABB MinimalEnclosingAABB() const;
int NumVertices() const;
Vector3 Vertex(int vertexIndex) const;
@@ -32,9 +36,6 @@ namespace J3ML::Geometry {
bool Intersects(const Frustum &frustum) const;
bool Intersects(const Polyhedron &polyhedron) const;
bool Intersects(const Sphere &sphere) const;
protected:
std::vector<Triangle> Triangulate() const;
/// Tests if this polygon is planar.
/** A polygon is planar if all its vertices lie on the same plane.
@@ -56,8 +57,31 @@ namespace J3ML::Geometry {
bool Contains(const Polygon &worldSpacePolygon, float polygonThickness) const;
bool Contains(const Vector3 &worldSpacePoint, float polygonThicknessSq) const;
bool Contains(const Vector3 &worldSpacePoint, float polygonThicknessSq = 1e-2f) const;
bool Contains(const LineSegment &worldSpaceLineSegment, float polygonThickness) const;
bool Contains(const Triangle &worldSpaceTriangle, float polygonThickness) const;
LineSegment Edge(int i) const;
bool ConvexIntersects(const AABB &aabb) const;
bool ConvexIntersects(const OBB &obb) const;
bool ConvexIntersects(const Frustum &frustum) const;
Vector3 MapFrom2D(const Vector2 &point) const;
bool Intersects2D(const LineSegment &segment) const;
Vector3 ClosestPoint(const LineSegment &lineSegment) const;
Vector3 ClosestPoint(const LineSegment &lineSegment, Vector3 *lineSegmentPt) const;
Vector3 ClosestPoint(const Vector3 &point) const;
protected:
};
}

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

@@ -66,9 +66,7 @@ namespace J3ML::Geometry
bool Intersects(const Polygon&) const;
bool Intersects(const Frustum&) const;
bool Intersects(const Sphere&) const;
bool Intersects(const Capsule&) const;
float FaceContainmentDistance2D(int i, Vector3 vector3) const;
bool Intersects(const Capsule& capsule) const;
bool IsClosed() const;
@@ -103,11 +101,11 @@ namespace J3ML::Geometry
/// 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;
float FaceContainmentDistance2D(int faceIndex, const Vector3 &worldSpacePoint, float polygonThickness = 1e-5f) const;
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
Vector3 ClosestPoint(const LineSegment& lineSegment, Vector3 *lineSegmentPt) const;
protected:
private:
};
}

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

@@ -42,6 +42,7 @@ namespace J3ML::Geometry
explicit Ray(const LineSegment& lineSegment);
bool IsFinite() const;
Vector3 GetPoint(float distance) const;
RaycastResult Cast(const Triangle& target, float maxDistance = 99999999);
RaycastResult Cast(const Plane& target, float maxDistance = 99999999);
RaycastResult Cast(const AABB& target, float maxDistance = 99999999);

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

@@ -18,8 +18,22 @@ namespace J3ML::Geometry
public:
Vector3 Position;
float Radius;
public:
Sphere() {}
Sphere(const Vector3& pos, float radius) : Position(pos), Radius(radius) {}
/// Quickly returns an arbitrary point inside this Sphere. Used in GJK intersection test.
Vector3 AnyPointFast() const { return Position; }
/// Computes the extreme point of this Sphere in the given direction.
/** An extreme point is a farthest point of this Sphere in the given direction. For
a Sphere, this point is unique.
@param direction The direction vector of the direction to find the extreme point. This vector may
be unnormalized, but may not be null.
@return The extreme point of this Sphere in the given direction. */
Vector3 ExtremePoint(const Vector3 &direction) const;
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
void Translate(const Vector3& offset)
{
Position = Position + offset;
@@ -56,6 +70,7 @@ namespace J3ML::Geometry
{
return Position.DistanceSquared(point) <= Radius*Radius;
}
bool Contains(const Vector3& point, float epsilon) const
{
return Position.DistanceSquared(point) <= Radius*Radius + epsilon;

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

@@ -2,6 +2,7 @@
#include <J3ML/Geometry/Common.h>
#include <J3ML/LinearAlgebra.h>
#include <cfloat>
namespace J3ML::Geometry
{
@@ -11,15 +12,39 @@ namespace J3ML::Geometry
Vector3 V0;
Vector3 V1;
Vector3 V2;
public:
float DistanceSq(const Vector3 &point) const;
bool Intersects(const AABB& aabb) const;
bool Intersects(const Capsule& capsule) const;
AABB BoundingAABB() const;
bool Contains(const Vector3&) const;
/// Tests if the given object is fully contained inside this triangle.
/** @param triangleThickness An epsilon threshold value to use for this test. triangleThicknessSq is the squared version of this parameter.
This specifies the maximum distance the given object can lie from the plane defined by this triangle.
@see Distance(), Intersects(), ClosestPoint().
@todo Add Triangle::Contains(Circle) and Triangle::Contains(Disc). */
bool Contains(const Vector3& point, float triangleThicknessSq = 1e-5f) const;
bool Contains(const LineSegment& lineSeg, float triangleThickness = 1e-3f) const;
bool Contains(const Triangle& triangle, float triangleThickness = 1e-3f) const;
void ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const;
/// Quickly returns an arbitrary point inside this Triangle. Used in GJK intersection test.
inline Vector3 AnyPointFast() const { return V0; }
/// Computes an extreme point of this Triangle in the given direction.
/** An extreme point is a farthest point of this Triangle in the given direction. Given a direction,
this point is not necessarily unique.
@param direction The direction vector of the direction to find the extreme point. This vector may
be unnormalized, but may not be null.
@return An extreme point of this Triangle in the given direction. The returned point is always a
vertex of this Triangle.
@see Vertex(). */
Vector3 ExtremePoint(const Vector3 &direction) const;
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
static float IntersectLineTri(const Vector3 &linePos, const Vector3 &lineDir, const Vector3 &v0, const Vector3 &v1,
const Vector3 &v2, float &u, float &v);

2
include/J3ML/J3ML.h
View File

@@ -43,7 +43,7 @@ namespace J3ML::Math
{
bool EqualAbs(float a, float b, float epsilon = 1e-3f);
float RecipFast(float x);
// Coming soon: Units Namespace
// For Dimensional Analysis
/*

13
include/J3ML/LinearAlgebra/Matrix4x4.h
View File

@@ -121,10 +121,19 @@ namespace J3ML::LinearAlgebra {
void SetRow(int row, const Vector4& rowVector);
void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);
void SetCol(int col, const Vector3& colVector, float m_c3);
void SetCol(int col, const Vector4& colVector);
void SetCol(int col, float m_c0, float m_c1, float m_c2, float m_c3);
void SetCol(int col, const float *data);
Vector4 GetRow(int index) const;
Vector4 GetColumn(int index) const;
Vector3 GetRow3(int index) const;
Vector3 GetColumn3(int index) const;
Vector4 Col(int i) const;
Vector4 Row(int i) const;
Vector4 Col3(int i) const;
Vector4 Row3(int i) const;
Vector3 GetScale() const
{
@@ -264,9 +273,13 @@ namespace J3ML::LinearAlgebra {
/// Returns true if this matrix is seen to contain a "projective" part,
/// i.e. whether the last row of this matrix differs from [0 0 0 1]
bool ContainsProjection(float epsilon = 1e-3f) const;
void InverseOrthonormal();
protected:
float elems[4][4];
Vector3 TransformDir(float tx, float ty, float tz) const;
};
}

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

@@ -11,6 +11,8 @@ namespace J3ML::LinearAlgebra {
// A 3D (x, y, z) ordered pair.
class Vector3 {
public:
float x = 0;
float y = 0;
float z = 0;
@@ -36,6 +38,7 @@ public:
Vector3();
// Constructs a new Vector3 with the value (X, Y, Z)
Vector3(float X, float Y, float Z);
Vector3(const Vector2& XY, float Z);
Vector3(const Vector3& rhs); // Copy Constructor
Vector3(Vector3&&) = default; // Move Constructor
/// Constructs this float3 from a C array, to the value (data[0], data[1], data[2]).

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

@@ -42,7 +42,18 @@ namespace J3ML::LinearAlgebra {
float &operator[](std::size_t index);
bool IsWithinMarginOfError(const Vector4& rhs, float margin=0.0001f) const;
bool IsNormalized(float epsilonSq = 1e-5f) const;
float LengthSqXYZ() const;
bool IsNormalized3(float epsilonSq = 1e-5f) const
{
return std::abs(LengthSqXYZ()-1.f) <= epsilonSq;
}
bool IsNormalized4(float epsilonSq = 1e-5f) const
{
return std::abs(LengthSquared()-1.f) <= epsilonSq;
}
bool IsNormalized(float epsilonSq = 1e-5f) const { return IsNormalized4(epsilonSq); }
bool IsZero(float epsilonSq = 1e-6f) const;
bool IsFinite() const;
bool IsPerpendicular(const Vector4& other, float epsilonSq=1e-5f) const
@@ -74,7 +85,9 @@ namespace J3ML::LinearAlgebra {
// the cross product only has the orthogonality property in 3 and 7 dimensions
// You should consider instead looking at Gram-Schmidt Orthogonalization
// to find orthonormal vectors.
Vector4 Cross(const Vector4& rhs) const;
Vector4 Cross3(const Vector3& rhs) const;
Vector4 Cross3(const Vector4& rhs) const;
Vector4 Cross(const Vector4& rhs) const { return Cross3(rhs); }
Vector4 Normalize() const;
Vector4 Lerp(const Vector4& goal, float alpha) const;

331
src/J3ML/Algorithm/GJK.cpp Normal file
View File

@@ -0,0 +1,331 @@
#include <J3ML/Algorithm/GJK.h>
#include <J3ML/Geometry.h>
namespace J3ML::Algorithms
{
/// This function examines the simplex defined by the array of points in s, and calculates which voronoi region
/// of that simplex the origin is closest to. Based on that information, the function constructs a new simplex
/// that will be used to continue the search, and returns a new search direction for the GJK algorithm.
/// @param s [in, out] An array of points in the simplex. When this function returns, this point array is updates to contain the new search simplex.
/// @param n [in, out] The number of points in the array s. When this function returns, this reference is updated to specify how many points the new search simplex contains.
/// @return The new search direction vector.
Vector3 UpdateSimplex(Vector3 *s, int &n) {
if (n == 2)
{
// Four voronoi regions that the origin could be in
// 0) closest to vertex s[0]
// 1) closest to vertex s[1]
// 2) closest to line segment s[0]->s[1]. XX
// 3) contained in the line segment s[0]->s[1], and our search is over and the algorithm is now finished. XX
// By construction of the simplex, the cases 0) and 1) can never occur. Then only the cases marked with XX need to be checked.
// Sanity-check that the above reasoning is valid by testing each voronoi region and asserting that the ones we assume never to happen never will
float d0 = s[0].DistanceSq(Vector3::Zero);
float d1 = s[1].DistanceSq(Vector3::Zero);
float d2 = LineSegment(s[0], s[1]).DistanceSq(Vector3::Zero);
assert(d2 <= d0);
assert(d2 <= d1);
// Cannot be in case 0: the step 0 -> 1 must have been toward the zero direction
assert(Vector3::Dot(s[1]-s[0], -s[0]) >= 0.f);
// Cannot be in case 1: the zero direction cannot be in the voronoi region of vertex s[1].
assert(Vector3::Dot(s[1]-s[0], -s[1]) <= 0.f);
Vector3 d01 = s[1] - s[0];
Vector3 newSearchDir = Vector3::Cross(d01, Vector3::Cross(d01, s[1]));
if (newSearchDir.LengthSquared() > 1e-7f)
return newSearchDir;
else { // Case 3
n = 0;
return Vector3::Zero;
}
}
else if (n == 3)
{
// Nine voronoi regions:
// 0) closest to vertex s[0].
// 1) closest to vertex s[1].
// 2) closest to vertex s[2].
// 3) closest to edge s[0]->s[1].
// 4) closest to edge s[1]->s[2]. XX
// 5) closest to edge s[0]->s[2]. XX
// 6) closest to the triangle s[0]->s[1]->s[2], in the positive side. XX
// 7) closest to the triangle s[0]->s[1]->s[2], in the negative side. XX
// 8) contained in the triangle s[0]->s[1]->s[2], and our search is over and the algorithm is now finished. XX
// By construction of the simplex, the origin must always be in a voronoi region that includes the point s[2], since that
// was the last added point. But it cannot be the case 2), since previous search took us deepest towards the direction s[1]->s[2],
// and case 2) implies we should have been able to go even deeper in that direction, or that the origin is not included in the convex shape,
// a case which has been checked for already before. Therefore the cases 0)-3) can never occur. Only the cases marked with XX need to be checked.
// Sanity-check that the above reasoning is valid by testing each voronoi region and assert()ing that the ones we assume never to
// happen never will.
float d[7];
d[0] = s[0].DistanceSq(Vector3::Zero);
d[1] = s[1].DistanceSq(Vector3::Zero);
d[2] = s[2].DistanceSq(Vector3::Zero);
d[3] = LineSegment(s[0], s[1]).DistanceSq(Vector3::Zero);
d[4] = LineSegment(s[1], s[2]).DistanceSq(Vector3::Zero);
d[5] = LineSegment(s[2], s[0]).DistanceSq(Vector3::Zero);
d[6] = Triangle(s[0], s[1], s[2]).DistanceSq(Vector3::Zero);
bool isContainedInTriangle = (d[6] <= 1e-3f); // Are we in case 8)?
float dist = INFINITY;
int minDistIndex = -1;
for(int i = 4; i < 7; ++i)
if (d[i] < dist)
{
dist = d[i];
minDistIndex = i;
}
assert(isContainedInTriangle || dist <= d[0] + 1e-4f);
assert(isContainedInTriangle || dist <= d[1] + 1e-4f);
assert(isContainedInTriangle || dist <= d[2] + 1e-4f);
assert(isContainedInTriangle || dist <= d[3] + 1e-4f);
Vector3 d12 = s[2]-s[1];
Vector3 d02 = s[2]-s[0];
Vector3 triNormal = Vector3::Cross(d02, d12);
Vector3 e12 = Vector3::Cross(d12, triNormal);
float t12 = Vector3::Dot(s[1], e12);
if (t12 < 0.f)
{
// Case 4: Edge 1->2 is closest.
//assert(d[4] <= dist + 1e-3f * Max(1.f, d[4], dist));
Vector3 newDir = Vector3::Cross(d12, Vector3::Cross(d12, s[1]));
s[0] = s[1];
s[1] = s[2];
n = 2;
return newDir;
}
Vector3 e02 = Vector3::Cross(triNormal, d02);
float t02 = Vector3::Dot(s[0], e02);
if (t02 < 0.f)
{
// Case 5: Edge 0->2 is closest.
//assert(d[5] <= dist + 1e-3f * Max(1.f, d[5], dist));
Vector3 newDir = Vector3::Cross(d02, Vector3::Cross(d02, s[0]));
s[1] = s[2];
n = 2;
return newDir;
}
// Cases 6)-8):
//assert(d[6] <= dist + 1e-3f * Max(1.f, d[6], dist));
float scaledSignedDistToTriangle = triNormal.Dot(s[2]);
float distSq = scaledSignedDistToTriangle*scaledSignedDistToTriangle;
float scaledEpsilonSq = 1e-6f*triNormal.LengthSquared();
if (distSq > scaledEpsilonSq)
{
// The origin is sufficiently far away from the triangle.
if (scaledSignedDistToTriangle <= 0.f)
return triNormal; // Case 6)
else
{
// Case 7) Swap s[0] and s[1] so that the normal of Triangle(s[0],s[1],s[2]).PlaneCCW() will always point towards the new search direction.
std::swap(s[0], s[1]);
return -triNormal;
}
}
else
{
// Case 8) The origin lies directly inside the triangle. For robustness, terminate the search here immediately with success.
n = 0;
return Vector3::Zero;
}
}
else // n == 4
{
// A tetrahedron defines fifteen voronoi regions:
// 0) closest to vertex s[0].
// 1) closest to vertex s[1].
// 2) closest to vertex s[2].
// 3) closest to vertex s[3].
// 4) closest to edge s[0]->s[1].
// 5) closest to edge s[0]->s[2].
// 6) closest to edge s[0]->s[3]. XX
// 7) closest to edge s[1]->s[2].
// 8) closest to edge s[1]->s[3]. XX
// 9) closest to edge s[2]->s[3]. XX
// 10) closest to the triangle s[0]->s[1]->s[2], in the outfacing side.
// 11) closest to the triangle s[0]->s[1]->s[3], in the outfacing side. XX
// 12) closest to the triangle s[0]->s[2]->s[3], in the outfacing side. XX
// 13) closest to the triangle s[1]->s[2]->s[3], in the outfacing side. XX
// 14) contained inside the tetrahedron simplex, and our search is over and the algorithm is now finished. XX
// By construction of the simplex, the origin must always be in a voronoi region that includes the point s[3], since that
// was the last added point. But it cannot be the case 3), since previous search took us deepest towards the direction s[2]->s[3],
// and case 3) implies we should have been able to go even deeper in that direction, or that the origin is not included in the convex shape,
// a case which has been checked for already before. Therefore the cases 0)-5), 7) and 10) can never occur and
// we only need to check cases 6), 8), 9), 11), 12), 13) and 14), marked with XX.
#ifdef MATH_ASSERT_CORRECTNESS
// Sanity-check that the above reasoning is valid by testing each voronoi region and assert()ing that the ones we assume never to
// happen never will.
double d[14];
d[0] = POINT_TO_FLOAT4(s[0]).Distance4Sq(POINT_TO_FLOAT4(vec::zero));
d[1] = POINT_TO_FLOAT4(s[1]).Distance4Sq(POINT_TO_FLOAT4(vec::zero));
d[2] = POINT_TO_FLOAT4(s[2]).Distance4Sq(POINT_TO_FLOAT4(vec::zero));
d[3] = POINT_TO_FLOAT4(s[3]).Distance4Sq(POINT_TO_FLOAT4(vec::zero));
d[4] = LineSegment(s[0], s[1]).DistanceSqD(vec::zero);
d[5] = LineSegment(s[0], s[2]).DistanceSqD(vec::zero);
d[6] = LineSegment(s[0], s[3]).DistanceSqD(vec::zero);
d[7] = LineSegment(s[1], s[2]).DistanceSqD(vec::zero);
d[8] = LineSegment(s[1], s[3]).DistanceSqD(vec::zero);
d[9] = LineSegment(s[2], s[3]).DistanceSqD(vec::zero);
d[10] = Triangle(s[0], s[1], s[2]).DistanceSqD(vec::zero);
d[11] = Triangle(s[0], s[1], s[3]).DistanceSqD(vec::zero);
d[12] = Triangle(s[0], s[2], s[3]).DistanceSqD(vec::zero);
d[13] = Triangle(s[1], s[2], s[3]).DistanceSqD(vec::zero);
vec Tri013Normal = Cross(s[1]-s[0], s[3]-s[0]);
vec Tri023Normal = Cross(s[3]-s[0], s[2]-s[0]);
vec Tri123Normal = Cross(s[2]-s[1], s[3]-s[1]);
vec Tri012Normal = Cross(s[2] - s[0], s[1] - s[0]);
assert(Dot(Tri012Normal, s[3] - s[0]) <= 0.f);
float InTri012 = Dot(-s[0], Tri012Normal);
float InTri013 = Dot(-s[3], Tri013Normal);
float InTri023 = Dot(-s[3], Tri023Normal);
float InTri123 = Dot(-s[3], Tri123Normal);
bool insideSimplex = InTri012 <= 0.f && InTri013 <= 0.f && InTri023 <= 0.f && InTri123 <= 0.f;
double dist = FLOAT_INF;
int minDistIndex = -1;
for(int i = 6; i < 14; ++i)
if (i == 6 || i == 8 || i == 9 || i == 11 || i == 12 || i == 13)
if (d[i] < dist)
{
dist = d[i];
minDistIndex = i;
}
assert4(insideSimplex || dist <= d[0] + 1e-4 * Max(1.0, d[0], dist), d[0], dist, insideSimplex, minDistIndex);
assert4(insideSimplex || dist <= d[1] + 1e-4 * Max(1.0, d[1], dist), d[1], dist, insideSimplex, minDistIndex);
assert4(insideSimplex || dist <= d[2] + 1e-4 * Max(1.0, d[2], dist), d[2], dist, insideSimplex, minDistIndex);
assert4(insideSimplex || dist <= d[4] + 1e-4 * Max(1.0, d[4], dist), d[4], dist, insideSimplex, minDistIndex);
assert4(insideSimplex || dist <= d[5] + 1e-4 * Max(1.0, d[5], dist), d[5], dist, insideSimplex, minDistIndex);
assert4(insideSimplex || dist <= d[7] + 1e-4 * Max(1.0, d[7], dist), d[7], dist, insideSimplex, minDistIndex);
assert4(insideSimplex || dist <= d[10] + 1e-4 * Max(1.0, d[10], dist), d[10], dist, insideSimplex, minDistIndex);
#endif
Vector3 d01 = s[1] - s[0];
Vector3 d02 = s[2] - s[0];
Vector3 d03 = s[3] - s[0];
Vector3 tri013Normal = Vector3::Cross(d01, d03); // Normal of triangle 0->1->3 pointing outwards from the simplex.
Vector3 tri023Normal = Vector3::Cross(d03, d02); // Normal of triangle 0->2->3 pointing outwards from the simplex.
#ifdef MATH_ASSERT_CORRECTNESS
float4d D01 = POINT_TO_FLOAT4(s[1]) - POINT_TO_FLOAT4(s[0]);
float4d D02 = POINT_TO_FLOAT4(s[2]) - POINT_TO_FLOAT4(s[0]);
float4d D03 = POINT_TO_FLOAT4(s[3]) - POINT_TO_FLOAT4(s[0]);
float4d tri013NormalD = D01.Cross(D03);
float4d tri023NormalD = D03.Cross(D02);
assert3(tri013NormalD.Dot(D02) <= 0.f, tri013NormalD, D02, tri013NormalD.Dot(D02));
assert3(tri023NormalD.Dot(D01) <= 0.f, tri023NormalD, D01, tri023NormalD.Dot(D01));
#endif
Vector3 e03_1 = Vector3::Cross(tri013Normal, d03); // The normal of edge 0->3 on triangle 013.
Vector3 e03_2 = Vector3::Cross(d03, tri023Normal); // The normal of edge 0->3 on triangle 023.
float inE03_1 = Vector3::Dot(e03_1, s[3]);
float inE03_2 = Vector3::Dot(e03_2, s[3]);
if (inE03_1 <= 0.f && inE03_2 <= 0.f)
{
// Case 6) Edge 0->3 is closest. Simplex degenerates to a line segment.
#ifdef MATH_ASSERT_CORRECTNESS
assert4(!insideSimplex && d[6] <= dist + 1e-3f * Max(1.0, d[6], dist), d[6], dist, insideSimplex, minDistIndex);
#endif
Vector3 newDir = Vector3::Cross(d03, Vector3::Cross(d03, s[3]));
s[1] = s[3];
n = 2;
return newDir;
}
Vector3 d12 = s[2] - s[1];
Vector3 d13 = s[3] - s[1];
Vector3 tri123Normal = Vector3::Cross(d12, d13);
assert(Vector3::Dot(tri123Normal, -d02) <= 0.f);
Vector3 e13_0 = Vector3::Cross(d13, tri013Normal);
Vector3 e13_2 = Vector3::Cross(tri123Normal, d13);
float inE13_0 = Vector3::Dot(e13_0, s[3]);
float inE13_2 = Vector3::Dot(e13_2, s[3]);
if (inE13_0 <= 0.f && inE13_2 <= 0.f)
{
// Case 8) Edge 1->3 is closest. Simplex degenerates to a line segment.
#ifdef MATH_ASSERT_CORRECTNESS
assert4(!insideSimplex && d[8] <= dist + 1e-3f * Max(1.0, d[8], dist), d[8], dist, insideSimplex, minDistIndex);
#endif
Vector3 newDir = Vector3::Cross(d13, Vector3::Cross(d13, s[3]));
s[0] = s[1];
s[1] = s[3];
n = 2;
return newDir;
}
Vector3 d23 = s[3] - s[2];
Vector3 e23_0 = Vector3::Cross(tri023Normal, d23);
Vector3 e23_1 = Vector3::Cross(d23, tri123Normal);
float inE23_0 = Vector3::Dot(e23_0, s[3]);
float inE23_1 = Vector3::Dot(e23_1, s[3]);
if (inE23_0 <= 0.f && inE23_1 <= 0.f)
{
// Case 9) Edge 2->3 is closest. Simplex degenerates to a line segment.
#ifdef MATH_ASSERT_CORRECTNESS
assert4(!insideSimplex && d[9] <= dist + 1e-3f * Max(1.0, d[9], dist), d[9], dist, insideSimplex, minDistIndex);
#endif
Vector3 newDir = Vector3::Cross(d23, Vector3::Cross(d23, s[3]));
s[0] = s[2];
s[1] = s[3];
n = 2;
return newDir;
}
float inTri013 = Vector3::Dot(s[3], tri013Normal);
if (inTri013 < 0.f && inE13_0 >= 0.f && inE03_1 >= 0.f)
{
// Case 11) Triangle 0->1->3 is closest.
#ifdef MATH_ASSERT_CORRECTNESS
assert4(!insideSimplex && d[11] <= dist + 1e-3f * Max(1.0, d[11], dist), d[11], dist, insideSimplex, minDistIndex);
#endif
s[2] = s[3];
n = 3;
return tri013Normal;
}
float inTri023 = Vector3::Dot(s[3], tri023Normal);
if (inTri023 < 0.f && inE23_0 >= 0.f && inE03_2 >= 0.f)
{
// Case 12) Triangle 0->2->3 is closest.
#ifdef MATH_ASSERT_CORRECTNESS
assert4(!insideSimplex && d[12] <= dist + 1e-3f * Max(1.0, d[12], dist), d[12], dist, insideSimplex, minDistIndex);
#endif
s[1] = s[0];
s[0] = s[2];
s[2] = s[3];
n = 3;
return tri023Normal;
}
float inTri123 = Vector3::Dot(s[3], tri123Normal);
if (inTri123 < 0.f && inE13_2 >= 0.f && inE23_1 >= 0.f)
{
// Case 13) Triangle 1->2->3 is closest.
#ifdef MATH_ASSERT_CORRECTNESS
assert4(!insideSimplex && d[13] <= dist + 1e-3f * Max(1.0, d[13], dist), d[13], dist, insideSimplex, minDistIndex);
#endif
s[0] = s[1];
s[1] = s[2];
s[2] = s[3];
n = 3;
return tri123Normal;
}
// Case 14) Not in the voronoi region of any triangle or edge. The origin is contained in the simplex, the search is finished.
n = 0;
return Vector3::Zero;
}
}
}

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

@@ -111,7 +111,7 @@ namespace J3ML::Geometry {
direction.z >= 0.f ? maxPoint.z : minPoint.z};
}
Vector3 AABB::ExtremePoint(const Vector3 &direction, float &projectionDistance) {
Vector3 AABB::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
auto extremePt = ExtremePoint(direction);
projectionDistance = extremePt.Dot(direction);
return extremePt;
@@ -326,6 +326,9 @@ namespace J3ML::Geometry {
Vector3 u2 = Vector3(0.f, 0.f, 1.f);
bool AABB::TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const
{
@@ -434,6 +437,93 @@ namespace J3ML::Geometry {
return false;
}
bool AABB::Intersects(const LineSegment &lineSegment) const
{
Vector3 dir = lineSegment.B - lineSegment.A;
float len = dir.Length();
if (len <= 1e-4f) // Degenerate line segment? Fall back to point-in-AABB test.
return Contains(lineSegment.A);
float invLen = 1.f / len;
dir *= invLen;
float tNear = 0.f, tFar = len;
#ifdef MATH_SIMD
return IntersectLineAABB_SSE(lineSegment.a, dir, tNear, tFar);
#else
return IntersectLineAABB_CPP(lineSegment.A, dir, tNear, tFar);
#endif
}
bool AABB::IntersectLineAABB_CPP(const Vector3 &linePos, const Vector3 &lineDir, float &tNear, float &tFar) const
{
assert(lineDir.IsNormalized());
assert(tNear <= tFar && "AABB::IntersectLineAABB: User gave a degenerate line as input for the intersection test!");
// The user should have inputted values for tNear and tFar to specify the desired subrange [tNear, tFar] of the line
// for this intersection test.
// For a Line-AABB test, pass in
// tNear = -FLOAT_INF;
// tFar = FLOAT_INF;
// For a Ray-AABB test, pass in
// tNear = 0.f;
// tFar = FLOAT_INF;
// For a LineSegment-AABB test, pass in
// tNear = 0.f;
// tFar = LineSegment.Length();
// Test each cardinal plane (X, Y and Z) in turn.
if (!Math::EqualAbs(lineDir.x, 0.f))
{
float recipDir = Math::RecipFast(lineDir.x);
float t1 = (minPoint.x - linePos.x) * recipDir;
float t2 = (maxPoint.x - linePos.x) * recipDir;
// tNear tracks distance to intersect (enter) the AABB.
// tFar tracks the distance to exit the AABB.
if (t1 < t2)
tNear = Max(t1, tNear), tFar = std::min(t2, tFar);
else // Swap t1 and t2.
tNear = Max(t2, tNear), tFar = std::min(t1, tFar);
if (tNear > tFar)
return false; // Box is missed since we "exit" before entering it.
}
else if (linePos.x < minPoint.x || linePos.x > maxPoint.x)
return false; // The ray can't possibly enter the box, abort.
if (!Math::EqualAbs(lineDir.y, 0.f))
{
float recipDir = Math::RecipFast(lineDir.y);
float t1 = (minPoint.y - linePos.y) * recipDir;
float t2 = (maxPoint.y - linePos.y) * recipDir;
if (t1 < t2)
tNear = Max(t1, tNear), tFar = std::min(t2, tFar);
else // Swap t1 and t2.
tNear = Max(t2, tNear), tFar = std::min(t1, tFar);
if (tNear > tFar)
return false; // Box is missed since we "exit" before entering it.
}
else if (linePos.y < minPoint.y || linePos.y > maxPoint.y)
return false; // The ray can't possibly enter the box, abort.
if (!Math::EqualAbs(lineDir.z, 0.f)) // ray is parallel to plane in question
{
float recipDir = Math::RecipFast(lineDir.z);
float t1 = (minPoint.z - linePos.z) * recipDir;
float t2 = (maxPoint.z - linePos.z) * recipDir;
if (t1 < t2)
tNear = Max(t1, tNear), tFar = std::min(t2, tFar);
else // Swap t1 and t2.
tNear = Max(t2, tNear), tFar = std::min(t1, tFar);
}
else if (linePos.z < minPoint.z || linePos.z > maxPoint.z)
return false; // The ray can't possibly enter the box, abort.
return tNear <= tFar;
}
Vector3 AABB::AnyPointFast() const { return minPoint;}
Vector3 AABB::GetRandomPointOnSurface(RNG &rng) const {

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

@@ -92,4 +92,16 @@ namespace J3ML::Geometry
{
return polyhedron.Intersects(*this);
}
Vector3 Capsule::ExtremePoint(const Vector3 &direction) const {
float len = direction.Length();
assert(len > 0.f);
return (Vector3::Dot(direction, l.B - l.A) >= 0.f ? l.B : l.A) + direction * (r / len);
}
Vector3 Capsule::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
Vector3 extremePoint = ExtremePoint(direction);
projectionDistance = extremePoint.Dot(direction);
return extremePoint;
}
}

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

@@ -3,6 +3,10 @@
#include <cmath>
#include "J3ML/Geometry/AABB.h"
#include <J3ML/Geometry/Polyhedron.h>
#include <J3ML/Geometry/LineSegment.h>
#include <J3ML/Geometry/Polygon.h>
#include <J3ML/Algorithm/GJK.h>
namespace J3ML::Geometry
{
@@ -71,6 +75,45 @@ namespace J3ML::Geometry
}
}
LineSegment Frustum::Edge(int edgeIndex) const
{
assert(0 <= edgeIndex && edgeIndex <= 11);
switch(edgeIndex)
{
default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
case 0: return {CornerPoint(0), CornerPoint(1)};
case 1: return LineSegment(CornerPoint(0), CornerPoint(2));
case 2: return LineSegment(CornerPoint(0), CornerPoint(4));
case 3: return LineSegment(CornerPoint(1), CornerPoint(3));
case 4: return LineSegment(CornerPoint(1), CornerPoint(5));
case 5: return LineSegment(CornerPoint(2), CornerPoint(3));
case 6: return LineSegment(CornerPoint(2), CornerPoint(6));
case 7: return LineSegment(CornerPoint(3), CornerPoint(7));
case 8: return LineSegment(CornerPoint(4), CornerPoint(5));
case 9: return LineSegment(CornerPoint(4), CornerPoint(6));
case 10: return LineSegment(CornerPoint(5), CornerPoint(7));
case 11: return LineSegment(CornerPoint(6), CornerPoint(7));
}
}
Vector3 Frustum::ExtremePoint(const Vector3 &direction, float &projectionDistance) const
{
Vector3 corners[8];
GetCornerPoints(corners);
Vector3 mostExtreme = Vector3::NaN;
projectionDistance = -INFINITY;
for(int i = 0; i < 8; ++i)
{
float d = Vector3::Dot(direction, corners[i]);
if (d > projectionDistance)
{
projectionDistance = d;
mostExtreme = corners[i];
}
}
return mostExtreme;
}
Frustum Frustum::CreateFrustumFromCamera(const CoordinateFrame &cam, float aspect, float fovY, float zNear, float zFar) {
Frustum frustum;
const float halfVSide = zFar * tanf(fovY * 0.5f);
@@ -194,4 +237,60 @@ namespace J3ML::Geometry
bool Frustum::Intersects(const Ray &ray) const {
return this->ToPolyhedron().Intersects(ray);
}
bool Frustum::Intersects(const Line &line) const
{
///@todo This is a naive test. Implement a faster version.
return this->ToPolyhedron().Intersects(line);
}
bool Frustum::Intersects(const LineSegment &lineSegment) const
{
return Algorithms::GJKIntersect(*this, lineSegment);
}
bool Frustum::Intersects(const AABB &aabb) const
{
return Algorithms::GJKIntersect(*this, aabb);
}
bool Frustum::Intersects(const OBB &obb) const
{
return Algorithms::GJKIntersect(*this, obb);
}
bool Frustum::Intersects(const Plane &plane) const
{
return plane.Intersects(*this);
}
bool Frustum::Intersects(const Triangle &triangle) const
{
return Algorithms::GJKIntersect(*this, triangle);
}
bool Frustum::Intersects(const Polygon &polygon) const
{
return polygon.Intersects(*this);
}
bool Frustum::Intersects(const Sphere &sphere) const
{
return Algorithms::GJKIntersect(*this, sphere);
}
bool Frustum::Intersects(const Capsule &capsule) const
{
return Algorithms::GJKIntersect(*this, capsule);
}
bool Frustum::Intersects(const Frustum &frustum) const
{
return Algorithms::GJKIntersect(*this, frustum);
}
bool Frustum::Intersects(const Polyhedron &polyhedron) const
{
return this->ToPolyhedron().Intersects(polyhedron);
}
}

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

@@ -208,4 +208,33 @@ namespace J3ML::Geometry {
{
return A.DistanceSq(B);
}
bool LineSegment::Intersects(const LineSegment &segment) const {
return false;
}
Vector3 LineSegment::ExtremePoint(const Vector3 &direction) const {
return Vector3::Dot(direction, B-A) >= 0.f ? B : A;
}
Vector3 LineSegment::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
Vector3 extremePoint = ExtremePoint(direction);
projectionDistance = extremePoint.Dot(direction);
return extremePoint;
}
bool LineSegment::Contains(const Vector3 &point, float distanceThreshold) const {
return ClosestPoint(point).DistanceSq(point) <= distanceThreshold;
}
Vector3 LineSegment::ClosestPoint(const Vector3 &point) const { float d; return ClosestPoint(point, d); }
float LineSegment::Distance(const LineSegment &other, float &d, float &d2) const {
ClosestPoint(other, d, d2);
return GetPoint(d).Distance(other.GetPoint(d2));
}
LineSegment operator*(const Matrix4x4 &transform, const LineSegment &l) {
return LineSegment(transform.Mul(l.A), transform.Mul(l.B));
}
}

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

@@ -391,4 +391,53 @@ namespace J3ML::Geometry
}
}
bool OBB::Intersects(const LineSegment &lineSegment) const {
AABB aabb(Vector3(0.f), Size());
LineSegment l = WorldToLocal() * lineSegment;
return aabb.Intersects(l);
}
Matrix4x4 OBB::WorldToLocal() const
{
Matrix4x4 m = LocalToWorld();
m.InverseOrthonormal();
return m;
}
Matrix4x4 OBB::LocalToWorld() const
{
// To produce a normalized local->world matrix, do the following.
/*
float3x4 m;
vec x = axis[0] * r.x;
vec y = axis[1] * r.y;
vec z = axis[2] * r.z;
m.SetCol(0, 2.f * x);
m.SetCol(1, 2.f * y);
m.SetCol(2, 2.f * z);
m.SetCol(3, pos - x - y - z);
return m;
*/
assert(axis[0].IsNormalized());
assert(axis[1].IsNormalized());
assert(axis[2].IsNormalized());
Matrix4x4 m; ///\todo sse-matrix
m.SetCol(0, axis[0].ptr());
m.SetCol(1, axis[1].ptr());
m.SetCol(2, axis[2].ptr());
Vector3 p = pos - axis[0] * r.x - axis[1] * r.y - axis[2] * r.z;
m.SetCol(3, p.ptr());
assert(m.Row3(0).IsNormalized());
assert(m.Row3(1).IsNormalized());
assert(m.Row3(2).IsNormalized());
assert(m.Col(0).IsPerpendicular(m.Col(1)));
assert(m.Col(0).IsPerpendicular(m.Col(2)));
assert(m.Col(1).IsPerpendicular(m.Col(2)));
return m;
}
}

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

@@ -225,6 +225,12 @@ namespace J3ML::Geometry
return false;
}
Vector3 Plane::Project(const Vector3 &point) const
{
Vector3 projected = point - (Normal.Dot(point) - distance) * Normal;
return projected;
}
LineSegment Plane::Project(const LineSegment &lineSegment) {
return LineSegment(Project(lineSegment.A), Project(lineSegment.B));
}

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

@@ -3,6 +3,7 @@
#include <J3ML/Geometry/Triangle.h>
#include "J3ML/Geometry/Plane.h"
#include "J3ML/Geometry/Line.h"
#include <J3ML/Algorithm/GJK.h>
namespace J3ML::Geometry {
AABB Polygon::MinimalEnclosingAABB() const {
@@ -146,6 +147,12 @@ namespace J3ML::Geometry {
return Vector2(Vector3::Dot(pt, basisU), Vector3::Dot(pt, basisV));
}
Vector3 Polygon::MapFrom2D(const Vector2 &point) const
{
assert(!vertices.empty());
return (Vector3)vertices[0] + point.x * BasisU() + point.y * BasisV();
}
// A(u) = a1 + u * (a2-a1).
// B(v) = b1 + v * (b2-b1).
// Returns (u,v).
@@ -201,76 +208,6 @@ namespace J3ML::Geometry {
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/
@@ -472,20 +409,49 @@ namespace J3ML::Geometry {
return false;
return Contains(ray.GetPoint(d));
}
bool Polygon::Intersects2D(const LineSegment &localSpaceLineSegment) const
{
if (vertices.size() < 3)
return false;
const Vector3 basisU = BasisU();
const Vector3 basisV = BasisV();
const Vector3 origin = vertices[0];
LineSegment edge;
edge.A = Vector3(Vector3::Dot(vertices.back(), basisU), Vector3::Dot(vertices.back(), basisV), 0); // map to 2D
for (int i = 0; i < (int)vertices.size(); ++i)
{
edge.B = Vector3(Vector3::Dot(vertices[i], basisU), Vector3::Dot(vertices[i], basisV), 0); // map to 2D
if (edge.Intersects(localSpaceLineSegment))
return true;
edge.A = edge.B;
}
// The line segment did not intersect with any of the polygon edges, so either the whole line segment is inside
// the polygon, or it is fully outside the polygon. Test one point of the line segment to determine which.
Vector2 xy = {
localSpaceLineSegment.A.x,
localSpaceLineSegment.A.y
};
return Contains(MapFrom2D(xy));
}
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);
float denom = Vector3::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)));
if (std::abs(denom) < 1e-4f) // The plane of the polygon and the line are planar? Do the test in 2D.
return Intersects2D(LineSegment(Vector3(MapTo2D(lineSegment.A), 0), Vector3(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;
float t = (plane.distance - Vector3::Dot(plane.Normal, lineSegment.A)) / denom;
if (t < 0.f || t > 1.f)
return false;
@@ -495,43 +461,43 @@ namespace J3ML::Geometry {
{
// 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 minD = INFINITY;
float maxD = -INFINITY;
for(size_t i = 0; i < vertices.size(); ++i)
{
float d = plane.SignedDistance(p[i]);
minD = Min(minD, d);
maxD = Max(maxD, d);
float d = plane.SignedDistance(vertices[i]);
minD = std::min(minD, d);
maxD = std::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);
return Algorithms::GJKIntersect(*this, aabb);
}
bool Polygon::ConvexIntersects(const OBB &obb) const
{
return GJKIntersect(*this, obb);
return Algorithms::GJKIntersect(*this, obb);
}
bool Polygon::ConvexIntersects(const Frustum &frustum) const
{
return GJKIntersect(*this, frustum);
return Algorithms::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.A = p.vertices.back();
for(size_t i = 0; i < p.vertices.size(); ++i)
{
l.b = p.p[i];
l.B = p.vertices[i];
if (c.Intersects(l))
return true;
l.a = l.b;
l.A = l.B;
}
// Check all the edges of the convex shape against the polygon.
@@ -574,6 +540,64 @@ namespace J3ML::Geometry {
return Convex_Intersects_Polygon(frustum, *this);
}
LineSegment Polygon::Edge(int i) const
{
if (vertices.empty())
return LineSegment(Vector3::NaN, Vector3::NaN);
if (vertices.size() == 1)
return LineSegment(vertices[0], vertices[0]);
return LineSegment(vertices[i], vertices[(i+1)%vertices.size()]);
}
Vector3 Polygon::ClosestPoint(const Vector3 &point) const
{
assert(IsPlanar());
std::vector<Triangle> tris = Triangulate();
Vector3 closestPt = Vector3::NaN;
float closestDist = FLT_MAX;
for(size_t i = 0; i < tris.size(); ++i)
{
Vector3 pt = ((Triangle)tris[i]).ClosestPoint(point);
float d = pt.DistanceSq(point);
if (d < closestDist)
{
closestPt = pt;
closestDist = d;
}
}
return closestPt;
}
Vector3 Polygon::ClosestPoint(const LineSegment &lineSegment) const
{
return ClosestPoint(lineSegment, 0);
}
Vector3 Polygon::ClosestPoint(const LineSegment &lineSegment, Vector3 *lineSegmentPt) const
{
std::vector<Triangle> tris = Triangulate();
Vector3 closestPt = Vector3::NaN;
Vector3 closestLineSegmentPt = Vector3::NaN;
float closestDist = FLT_MAX;
for(size_t i = 0; i < tris.size(); ++i)
{
Vector3 lineSegPt;
Vector3 pt = ((Triangle)tris[i]).ClosestPoint(lineSegment, &lineSegPt);
float d = pt.DistanceSq(lineSegPt);
if (d < closestDist)
{
closestPt = pt;
closestLineSegmentPt = lineSegPt;
closestDist = d;
}
}
if (lineSegmentPt)
*lineSegmentPt = closestLineSegmentPt;
return closestPt;
}
}

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

@@ -5,6 +5,7 @@
#include "J3ML/Geometry/Ray.h"
#include "J3ML/Geometry/Line.h"
#include <J3ML/Geometry/Polygon.h>
#include <J3ML/Geometry/Capsule.h>
#include <set>
#include <cfloat>
@@ -636,6 +637,45 @@ namespace J3ML::Geometry
return false;
}
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;
}
}

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

@@ -192,6 +192,16 @@ namespace J3ML::Geometry
else
return GetPoint(d);
}
Ray::Ray(const Vector3 &pos, const Vector3 &dir) {
Origin = pos;
Direction = dir;
}
Vector3 Ray::GetPoint(float distance) const {
assert(Direction.IsNormalized());
return Origin + distance * Direction;
}
}

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

@@ -13,4 +13,16 @@ namespace J3ML::Geometry
outMin = d - Radius;
outMax = d + Radius;
}
Vector3 Sphere::ExtremePoint(const Vector3 &direction) const {
float len = direction.Length();
assert(len > 0.f);
return Position + direction * (Radius / len);
}
Vector3 Sphere::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
Vector3 extremePoint = ExtremePoint(direction);
projectionDistance = extremePoint.Dot(direction);
return extremePoint;
}
}

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

@@ -350,4 +350,65 @@ namespace J3ML::Geometry
bool Triangle::Intersects(const Capsule &capsule) const {
return capsule.Intersects(*this);
}
Vector3 Triangle::ExtremePoint(const Vector3 &direction) const {
Vector3 mostExtreme = Vector3::NaN;
float mostExtremeDist = -FLT_MAX;
for(int i = 0; i < 3; ++i)
{
Vector3 pt = Vertex(i);
float d = Vector3::Dot(direction, pt);
if (d > mostExtremeDist)
{
mostExtremeDist = d;
mostExtreme = pt;
}
}
return mostExtreme;
}
Vector3 Triangle::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
Vector3 extremePoint = ExtremePoint(direction);
projectionDistance = extremePoint.Dot(direction);
return extremePoint;
}
float Triangle::DistanceSq(const Vector3 &point) const {
return ClosestPoint(point).DistanceSq(point);
}
Vector2 Triangle::BarycentricUV(const Vector3 &point) const {
Vector3 uvw = BarycentricUVW(point);
return Vector2{uvw.y, uvw.z};
}
Vector3 Triangle::Point(const Vector2 &uv) const {
return Point(uv.x, uv.y);
}
Vector3 Triangle::Point(float u, float v) const {
// In case the triangle is far away from the origin but is small in size, the elements of 'a' will have large magnitudes,
// and the elements of (b-a) and (c-a) will be much smaller quantities. Therefore be extra careful with the
// parentheses and first sum the small floats together before adding it to the large one.
return V0 + ((V1-V0) * u + (V2-V0) * v);
}
Vector3 Triangle::Point(const Vector3 &uvw) const {
return Point(uvw.x, uvw.y, uvw.z);
}
Vector3 Triangle::Point(float u, float v, float w) const {
return u * V0 + v * V1 + w * V2;
}
bool Triangle::Contains(const Vector3 &point, float triangleThicknessSq) const {
Vector3 normal = (V1-V0).Cross(V2-V0);
float lenSq = normal.LengthSquared();
float d = normal.Dot(V1 - point);
if (d*d > triangleThicknessSq * lenSq)
return false; ///@todo The plane-point distance test is omitted in Real-Time Collision Detection. p. 25. A bug in the book?
Vector3 br = BarycentricUVW(point);
return br.x >= -1e-3f && br.y >= -1e-3f && br.z >= -1e-3f; // Allow for a small epsilon to properly account for points very near the edges of the triangle.
}
}

5
src/J3ML/J3ML.cpp
View File

@@ -28,6 +28,11 @@ namespace J3ML
return std::abs(a - b) < epsilon;
}
float Math::RecipFast(float x) {
// TODO: Implement SSE rcp instruction.
return 1.f / x;
}
Math::Rotation::Rotation() : valueInRadians(0) {}
Math::Rotation::Rotation(float value) : valueInRadians(value) {}

60
src/J3ML/LinearAlgebra/Matrix4x4.cpp
View File

@@ -646,4 +646,64 @@ namespace J3ML::LinearAlgebra {
&& GetRow(1).IsPerpendicular(GetRow(2), epsilon);
}
Vector4 Matrix4x4::Col(int i) const { return GetColumn(i);}
Vector4 Matrix4x4::Row(int i) const { return GetRow(i);}
Vector4 Matrix4x4::Col3(int i) const { return GetColumn3(i);}
Vector4 Matrix4x4::Row3(int i) const { return GetRow3(i);}
Vector3 Matrix4x4::TransformDir(float tx, float ty, float tz) const
{
assert(!this->ContainsProjection()); // This function does not divide by w or output it, so cannot have projection.
return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz,
At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz,
At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz);
}
void Matrix4x4::InverseOrthonormal()
{
assert(!ContainsProjection());
// a) Transpose the top-left 3x3 part in-place to produce R^t.
Swap(elems[0][1], elems[1][0]);
Swap(elems[0][2], elems[2][0]);
Swap(elems[1][2], elems[2][1]);
// b) Replace the top-right 3x1 part by computing R^t(-T).
SetTranslatePart(TransformDir(-At(0, 3), -At(1, 3), -At(2, 3)));
}
void Matrix4x4::SetCol(int col, const float *data) {
assert(data != nullptr);
SetCol(col, data[0], data[1], data[2], data[3]);
}
void Matrix4x4::SetCol(int column, float m_0c, float m_1c, float m_2c, float m_3c)
{
assert(column >= 0);
assert(column < Cols);
assert(std::isfinite(m_0c));
assert(std::isfinite(m_1c));
assert(std::isfinite(m_2c));
assert(std::isfinite(m_3c));
At(0, column) = m_0c;
At(1, column) = m_1c;
At(2, column) = m_2c;
At(3, column) = m_3c;
}
void Matrix4x4::SetCol(int column, const Vector3 &columnVector, float m_3c)
{
SetCol(column, columnVector.x, columnVector.y, columnVector.z, m_3c);
}
void Matrix4x4::SetCol(int column, const Vector4 &columnVector)
{
SetCol(column, columnVector.x, columnVector.y, columnVector.z, columnVector.w);
}
}

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

@@ -481,5 +481,25 @@ namespace J3ML::LinearAlgebra {
z = d2;
}
Vector3::Vector3(const Vector2& XY, float Z) {
x = XY.x;
y = XY.y;
z = Z;
}
Vector3::Vector3(float scalar) {
x = scalar;
y = scalar;
z = scalar;
}
float Vector3::DistanceSquared(const Vector3 &to) const {
return (*this-to).LengthSquared();
}
float Vector3::DistanceSquared(const Vector3 &from, const Vector3 &to) {
return from.DistanceSquared(to);
}
}

17
src/J3ML/LinearAlgebra/Vector4.cpp
View File

@@ -182,6 +182,23 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
}
float Vector4::LengthSqXYZ() const {
return x*x * y*y * z*z;
}
Vector4 Vector4::Cross3(const Vector3 &rhs) const {
Vector4 dst;
dst.x = y * rhs.z - z * rhs.y;
dst.y = z * rhs.x - x * rhs.z;
dst.z = x * rhs.y - y * rhs.x;
dst.w = 0.f;
return dst;
}
Vector4 Vector4::Cross3(const Vector4 &rhs) const {
return Cross3(rhs.XYZ());
}
}
#pragma endregion