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

589 lines
19 KiB
C++
Raw Blame History

#include <J3ML/Geometry/Shape.hpp>
#include <J3ML/Geometry/AABB.hpp>
#include <J3ML/Geometry/LineSegment.hpp>
#include <J3ML/Geometry/Polyhedron.hpp>
#include <J3ML/Geometry/OBB.hpp>
#include <J3ML/Geometry/Sphere.hpp>
namespace J3ML::Geometry {
using namespace J3ML::Math;
Polyhedron OBB::ToPolyhedron() const {
// Note to maintainer: This function is an exact copy of AABB::ToPolyhedron() and Frustum::ToPolyhedron()
Polyhedron p;
// populate the corners of this OBB
// this will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++
for (int i = 0; i < 8; ++i)
{
p.v.push_back(CornerPoint(i));
}
// generate the 6 faces of this OBB.
const int faces[6][4] =
{
{0, 1, 3, 2}, // X-
{4, 6, 7, 5}, // X+
{0, 4, 5, 1}, // Y-
{7, 6, 2, 3}, // Y+
{0, 2, 6, 4}, // Z-
{1, 5, 7, 3} // Z+
};
for (int f = 0; f < 6; ++f)
{
Polyhedron::Face face;
for (int v = 0; v < 4; ++v)
{
face.v.push_back(faces[f][v]);
}
//p.f.push_back(face);
}
return p;
}
Sphere OBB::MinimalEnclosingSphere() const {
Sphere s;
s.Position = pos;
s.Radius = HalfDiagonal().Length();
return s;
}
Sphere OBB::MaximalContainedSphere() const {
Sphere s;
s.Position = pos;
s.Radius = r.MinElement();
return s;
}
bool OBB::IsFinite() const {
return pos.IsFinite() && r.IsFinite() && axis[0].IsFinite() && axis[1].IsFinite() && axis[2].IsFinite();
}
bool OBB::IsDegenerate() const
{
return !(r.x > 0.f && r.y > 0.f && r.z > 0.f);
}
Vector3 OBB::CenterPoint() const
{
return pos;
}
Vector3 OBB::PointInside(float x, float y, float z) const
{
assert(0.f <= x && x <= 1.f);
assert(0.f <= y && y <= 1.f);
assert(0.f <= z && z <= 1.f);
return pos + axis[0] * (2.f * r.x * x - r.x)
+ axis[1] * (2.f * r.y * y - r.y)
+ axis[2] * (2.f * r.z * z - r.z);
}
Vector3 OBB::PointInside(const Vector3& pt) const
{
return PointInside(pt.x, pt.y, pt.z);
}
LineSegment OBB::Edge(int edgeIndex) const
{
assert(0 <= edgeIndex && edgeIndex <= 11);
switch(edgeIndex)
{
default: // For release builds where Assert() is disabled, return always the first option if out-of-bounds
case 0: return LineSegment(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 OBB::CornerPoint(int cornerIndex) const
{
assert(0 <= cornerIndex && cornerIndex <= 7);
switch(cornerIndex)
{
default: // For release builds, return always the first option if out of bounds
case 0: return pos - r.x * axis[0] - r.y * axis[1] - r.z * axis[2];
case 1: return pos - r.x * axis[0] - r.y * axis[1] + r.z * axis[2];
case 2: return pos - r.x * axis[0] + r.y * axis[1] - r.z * axis[2];
case 3: return pos - r.x * axis[0] + r.y * axis[1] + r.z * axis[2];
case 4: return pos + r.x * axis[0] - r.y * axis[1] - r.z * axis[2];
case 5: return pos + r.x * axis[0] - r.y * axis[1] + r.z * axis[2];
case 6: return pos + r.x * axis[0] + r.y * axis[1] - r.z * axis[2];
case 7: return pos + r.x * axis[0] + r.y * axis[1] + r.z * axis[2];
}
}
Vector3 OBB::ExtremePoint(const Vector3& direction) const
{
Vector3 pt = pos;
pt += axis[0] * (Vector3::Dot(direction, axis[0]) >= 0.f ? r.x : -r.x);
pt += axis[1] * (Vector3::Dot(direction, axis[1]) >= 0.f ? r.y : -r.y);
pt += axis[2] * (Vector3::Dot(direction, axis[2]) >= 0.f ? r.z : -r.z);
return pt;
}
Vector3 OBB::ExtremePoint(const Vector3& direction, float &projectionDistance) const
{
Vector3 extremePoint = ExtremePoint(direction);
projectionDistance = extremePoint.Dot(direction);
return extremePoint;
}
void OBB::ProjectToAxis(const Vector3& direction, float& outMin, float& outMax) const
{
float x = std::abs(Vector3::Dot(direction, axis[0]) * r.x);
float y = std::abs(Vector3::Dot(direction, axis[1]) * r.y);
float z = std::abs(Vector3::Dot(direction, axis[2]) * r.z);
float pt = Vector3::Dot(direction, pos);
outMin = pt - x - y - z;
outMax = pt + x + y + z;
}
int OBB::UniqueFaceNormals(Vector3* out) const
{
out[0] = axis[0];
out[1] = axis[1];
out[2] = axis[2];
return 3;
}
int OBB::UniqueEdgeDirections(Vector3* out) const
{
out[0] = axis[0];
out[1] = axis[1];
out[2] = axis[2];
return 3;
}
Vector3 OBB::PointOnEdge(int edgeIndex, float u) const
{
assert(0 <= edgeIndex && edgeIndex <= 11);
assert(0 <= u && u <= 1.f);
edgeIndex = std::clamp(edgeIndex, 0, 11);
Vector3 d = axis[edgeIndex/4] * (2.f * u - 1.f) * r[edgeIndex/4];
switch(edgeIndex)
{
default:
case 0: return pos - r.y * axis[1] - r.z * axis[2] + d;
case 1: return pos - r.y * axis[1] + r.z * axis[2] + d;
case 2: return pos + r.y * axis[1] - r.z * axis[2] + d;
case 3: return pos + r.y * axis[1] + r.z * axis[2] + d;
case 4: return pos - r.x * axis[0] - r.z * axis[2] + d;
case 5: return pos - r.x * axis[0] + r.z * axis[2] + d;
case 6: return pos + r.x * axis[0] - r.z * axis[2] + d;
case 7: return pos + r.x * axis[0] + r.z * axis[2] + d;
case 8: return pos - r.x * axis[0] - r.y * axis[1] + d;
case 9: return pos - r.x * axis[0] + r.y * axis[1] + d;
case 10: return pos + r.x * axis[0] - r.y * axis[1] + d;
case 11: return pos + r.x * axis[0] + r.y * axis[1] + d;
}
}
Vector3 OBB::FaceCenterPoint(int faceIndex) const
{
assert(0 <= faceIndex && faceIndex <= 6);
switch(faceIndex)
{
default:
case 0: return pos - r.x * axis[0];
case 1: return pos + r.x * axis[0];
case 2: return pos - r.y * axis[1];
case 3: return pos + r.y * axis[1];
case 4: return pos - r.z * axis[2];
case 5: return pos + r.z * axis[2];
}
}
Vector3 OBB::FacePoint(int faceIndex, float u, float v) const
{
assert(0 <= faceIndex && faceIndex <= 5);
assert(0 <= u && u <= 1.f);
assert(0 <= v && v <= 1.f);
int uIdx = faceIndex/2;
int vIdx = (faceIndex/2 + 1) % 3;
Vector3 U = axis[uIdx] * (2.f * u - 1.f) * r[uIdx];
Vector3 V = axis[vIdx] * (2.f * v - 1.f) * r[vIdx];
switch(faceIndex)
{
default:
case 0: return pos - r.z * axis[2] + U + V;
case 1: return pos + r.z * axis[2] + U + V;
case 2: return pos - r.x * axis[0] + U + V;
case 3: return pos + r.x * axis[0] + U + V;
case 4: return pos - r.y * axis[1] + U + V;
case 5: return pos + r.y * axis[1] + U + V;
}
}
Plane OBB::FacePlane(int faceIndex) const
{
assert(0 <= faceIndex && faceIndex <= 5);
switch(faceIndex)
{
default:
case 0: return Plane(FaceCenterPoint(0), -axis[0]);
case 1: return Plane(FaceCenterPoint(1), axis[0]);
case 2: return Plane(FaceCenterPoint(2), -axis[1]);
case 3: return Plane(FaceCenterPoint(3), axis[1]);
case 4: return Plane(FaceCenterPoint(4), -axis[2]);
case 5: return Plane(FaceCenterPoint(5), axis[2]);
}
}
void OBB::GetCornerPoints(Vector3 *outPointArray) const
{
assert(outPointArray);
for (int i = 0; i < 8; ++i)
outPointArray[i] = CornerPoint(i);
}
void OBB::GetFacePlanes(Plane *outPlaneArray) const
{
assert(outPlaneArray);
for (int i = 0; i < 6; ++i)
outPlaneArray[i] = FacePlane(i);
}
void OBB::ExtremePointsAlongDirection(const Vector3& dir, const Vector3* pointArray, int numPoints, int &idxSmallest, int &idxLargest, float &smallestD, float &largestD)
{
assert(pointArray || numPoints == 0);
idxSmallest = idxLargest = 0;
smallestD = INFINITY;
largestD = -INFINITY;
for (int i = 0; i < numPoints; ++i)
{
float d = Vector3::Dot(pointArray[i], dir);
if (d < smallestD)
{
smallestD = d;
idxSmallest = i;
}
if (d > largestD)
{
largestD = d;
idxLargest = i;
}
}
}
Vector3 OBB::Size() const {
return r * 2.f;
}
Vector3 OBB::HalfSize() const {
return r;
}
Vector3 OBB::Diagonal() const {
return 2.f * HalfDiagonal();
}
Vector3 OBB::HalfDiagonal() const {
return axis[0] * r[0] + axis[1] * r[1] + axis[2] * r[2];
}
float OBB::Volume() const {
Vector3 size = Size();
return size.x*size.y*size.z;
}
float OBB::SurfaceArea() const {
const Vector3 size = Size();
return 2.f * (size.x*size.y + size.x*size.z + size.y*size.z);
}
template <typename Matrix>
void OBBSetFrom(OBB &obb, const AABB& aabb, const Matrix& m)
{
assert(m.IsColOrthogonal()); // We cannot convert transform an AABB to OBB if it gets sheared in the process.
assert(m.HasUniformScale()); // Nonuniform scale will produce shear as well
obb.pos = m.Mul(aabb.Centroid());
obb.r = aabb.HalfSize();
obb.axis[0] = Vector3(m.GetColumn3(0));
obb.axis[1] = Vector3(m.GetColumn3(1));
obb.axis[2] = Vector3(m.GetColumn3(2));
// If te matrix m contains scaling, propagate the scaling from the axis vectors to the half-length vectors,
// since we want to keep the axis vectors always normalized in our representations.
float matrixScale = obb.axis[0].LengthSquared();
matrixScale = std::sqrt(matrixScale);
obb.r *= matrixScale;
matrixScale = 1.f / matrixScale;
obb.axis[0] *= matrixScale;
obb.axis[1] *= matrixScale;
obb.axis[2] *= matrixScale;
Vector3::Orthonormalize(obb.axis[0], obb.axis[1], obb.axis[2]);
}
template <typename Matrix>
void OBBTransform(OBB& o, const Matrix& transform)
{
o.pos = transform.Mul(o.pos);
o.axis[0] = transform.Mul(o.r.x * o.axis[0]);
o.axis[1] = transform.Mul(o.r.y * o.axis[1]);
o.axis[2] = transform.Mul(o.r.z * o.axis[2]);
o.r.x = o.axis[0].Normalized().x;
o.r.y = o.axis[1].Normalized().y;
o.r.z = o.axis[2].Normalized().z;
}
void OBB::SetFrom(const AABB& aabb, const Matrix3x3 &transform) {
assert(transform.IsColOrthogonal());
OBBSetFrom(*this, aabb, transform);
}
void OBB::SetFrom(const AABB& aabb, const Matrix4x4 &transform) {
assert(transform.IsColOrthogonal3());
OBBSetFrom(*this, aabb, transform);
}
void OBB::SetFrom(const AABB& aabb, const Quaternion &transform) {
OBBSetFrom(*this, aabb, Matrix3x3(transform));
}
void OBB::Transform(const Matrix3x3 &transform) {
assert(transform.IsColOrthogonal());
OBBTransform(*this, transform);
}
void OBB::Transform(const Matrix4x4 &transform) {
assert(transform.IsColOrthogonal3());
OBBTransform(*this, transform);
}
void OBB::Transform(const Quaternion &transform) {
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;
}
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;
}
Vector3 OBB::ClosestPoint(const Vector3& targetPoint) const {
Vector3 d = targetPoint - pos;
Vector3 closestPoint = pos; // Start at the center point of the OBB.
for (int i = 0; i < 3; ++i) // Project the target onto the OBB axes and walk towards that point.
closestPoint += Clamp(Vector3::Dot(d, axis[i]), -r[i], r[i]) * axis[i];
return closestPoint;
}
void OBB::Enclose(const Vector3& point) {
Vector3 p = point - pos;
for (int i = 0; i < 3; ++i) {
assert(Math::EqualAbs(axis[i].Length(), 1.f));
float dist = p.Dot(axis[i]);
float distanceFromOBB = Math::Abs(dist) - r[i];
if (distanceFromOBB > 0.f) {
r[i] += distanceFromOBB * 0.5f;
if (dist > 0.f) // TODO: Optimize out this comparison!
pos += axis[i] * distanceFromOBB * 0.5f;
else
pos -= axis[i] * distanceFromOBB * 0.5f;
p = point - pos;
assert(Math::EqualAbs(Math::Abs(p.Dot(axis[i])), r[i], 1e-1f));
}
}
assert(Distance(point) <= 1e-3f);
}
float OBB::Distance(const Vector3& point) const {
Vector3 closestPoint = ClosestPoint(point);
return point.Distance(closestPoint);
}
bool OBB::Contains(const Vector3 &point) const {
Vector3 pt = point - pos;
return Abs(Vector3::Dot(pt, axis[0])) <= r[0] &&
Abs(Vector3::Dot(pt, axis[1])) <= r[1] &&
Abs(Vector3::Dot(pt, axis[2])) <= r[2];
}
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;
}
OBB::OBB(const Vector3 &pos, const Vector3 &radii, const Vector3 &axis0, const Vector3 &axis1, const Vector3 &axis2) {
this->pos = pos;
this->r = radii;
this->axis[0] = axis0;
this->axis[1] = axis1;
this->axis[2] = axis2;
}
AABB OBB::MinimalEnclosingAABB() const {
AABB aabb;
aabb.SetFrom(*this);
return aabb;
}
Vector3 OBB::MinPoint() const {
return {MinX(), MinY(), MinZ()};
}
float OBB::MinX() const {
auto c0 = CornerPoint(0);
auto c1 = CornerPoint(1);
auto c2 = CornerPoint(2);
auto c3 = CornerPoint(3);
auto c4 = CornerPoint(4);
auto c5 = CornerPoint(5);
auto c6 = CornerPoint(6);
auto c7 = CornerPoint(7);
return std::min({c0.x, c1.x, c2.x, c3.x, c4.x, c5.x, c6.x, c7.x});
}
float OBB::MinY() const {
auto c0 = CornerPoint(0);
auto c1 = CornerPoint(1);
auto c2 = CornerPoint(2);
auto c3 = CornerPoint(3);
auto c4 = CornerPoint(4);
auto c5 = CornerPoint(5);
auto c6 = CornerPoint(6);
auto c7 = CornerPoint(7);
return std::min({c0.y, c1.y, c2.y, c3.y, c4.y, c5.y, c6.y, c7.y});
}
float OBB::MinZ() const {
auto c0 = CornerPoint(0);
auto c1 = CornerPoint(1);
auto c2 = CornerPoint(2);
auto c3 = CornerPoint(3);
auto c4 = CornerPoint(4);
auto c5 = CornerPoint(5);
auto c6 = CornerPoint(6);
auto c7 = CornerPoint(7);
return std::min({c0.z, c1.z, c2.z, c3.z, c4.z, c5.z, c6.z, c7.z});
}
float OBB::MaxX() const {
auto c0 = CornerPoint(0);
auto c1 = CornerPoint(1);
auto c2 = CornerPoint(2);
auto c3 = CornerPoint(3);
auto c4 = CornerPoint(4);
auto c5 = CornerPoint(5);
auto c6 = CornerPoint(6);
auto c7 = CornerPoint(7);
return std::max({c0.x, c1.x, c2.x, c3.x, c4.x, c5.x, c6.x, c7.x});
}
float OBB::MaxY() const {
auto c0 = CornerPoint(0);
auto c1 = CornerPoint(1);
auto c2 = CornerPoint(2);
auto c3 = CornerPoint(3);
auto c4 = CornerPoint(4);
auto c5 = CornerPoint(5);
auto c6 = CornerPoint(6);
auto c7 = CornerPoint(7);
return std::max({c0.y, c1.y, c2.y, c3.y, c4.y, c5.y, c6.y, c7.y});
}
float OBB::MaxZ() const {
auto c0 = CornerPoint(0);
auto c1 = CornerPoint(1);
auto c2 = CornerPoint(2);
auto c3 = CornerPoint(3);
auto c4 = CornerPoint(4);
auto c5 = CornerPoint(5);
auto c6 = CornerPoint(6);
auto c7 = CornerPoint(7);
return std::max({c0.z, c1.z, c2.z, c3.z, c4.z, c5.z, c6.z, c7.z});
}
Vector3 OBB::MaxPoint() const {
return {MaxX(), MaxY(), MaxZ()};
}
}
Reference in New Issue View Git Blame Copy Permalink