354 lines
10 KiB
C++
354 lines
10 KiB
C++
// Copyright (c) 2012-2021 Wojciech Figat. All rights reserved.
|
|
|
|
#include "OrientedBoundingBox.h"
|
|
#include "BoundingSphere.h"
|
|
#include "BoundingBox.h"
|
|
#include "Ray.h"
|
|
#include "../Types/String.h"
|
|
|
|
OrientedBoundingBox::OrientedBoundingBox(const BoundingBox& bb)
|
|
{
|
|
const Vector3 center = bb.Minimum + (bb.Maximum - bb.Minimum) / 2.0f;
|
|
Extents = bb.Maximum - center;
|
|
Matrix::Translation(center, Transformation);
|
|
}
|
|
|
|
OrientedBoundingBox::OrientedBoundingBox(Vector3 points[], int32 pointCount)
|
|
{
|
|
ASSERT(points && pointCount > 0);
|
|
|
|
Vector3 minimum = points[0];
|
|
Vector3 maximum = points[0];
|
|
|
|
for (int32 i = 1; i < pointCount; i++)
|
|
{
|
|
Vector3::Min(minimum, points[i], minimum);
|
|
Vector3::Max(maximum, points[i], maximum);
|
|
}
|
|
|
|
const Vector3 center = minimum + (maximum - minimum) / 2.0f;
|
|
Extents = maximum - center;
|
|
Matrix::Translation(center, Transformation);
|
|
}
|
|
|
|
String OrientedBoundingBox::ToString() const
|
|
{
|
|
return String::Format(TEXT("{}"), *this);
|
|
}
|
|
|
|
void OrientedBoundingBox::GetCorners(Vector3 corners[8]) const
|
|
{
|
|
Vector3 xv = Vector3(Extents.X, 0, 0);
|
|
Vector3 yv = Vector3(0, Extents.Y, 0);
|
|
Vector3 zv = Vector3(0, 0, Extents.Z);
|
|
|
|
Vector3::TransformNormal(xv, Transformation, xv);
|
|
Vector3::TransformNormal(yv, Transformation, yv);
|
|
Vector3::TransformNormal(zv, Transformation, zv);
|
|
|
|
const Vector3 center = Transformation.GetTranslation();
|
|
|
|
corners[0] = center + xv + yv + zv;
|
|
corners[1] = center + xv + yv - zv;
|
|
corners[2] = center - xv + yv - zv;
|
|
corners[3] = center - xv + yv + zv;
|
|
corners[4] = center + xv - yv + zv;
|
|
corners[5] = center + xv - yv - zv;
|
|
corners[6] = center - xv - yv - zv;
|
|
corners[7] = center - xv - yv + zv;
|
|
}
|
|
|
|
Vector3 OrientedBoundingBox::GetSize() const
|
|
{
|
|
Vector3 xv = Vector3(Extents.X * 2, 0, 0);
|
|
Vector3 yv = Vector3(0, Extents.Y * 2, 0);
|
|
Vector3 zv = Vector3(0, 0, Extents.Z * 2);
|
|
|
|
Vector3::TransformNormal(xv, Transformation, xv);
|
|
Vector3::TransformNormal(yv, Transformation, yv);
|
|
Vector3::TransformNormal(zv, Transformation, zv);
|
|
|
|
return Vector3(xv.Length(), yv.Length(), zv.Length());
|
|
}
|
|
|
|
Vector3 OrientedBoundingBox::GetSizeSquared() const
|
|
{
|
|
Vector3 xv = Vector3(Extents.X * 2, 0, 0);
|
|
Vector3 yv = Vector3(0, Extents.Y * 2, 0);
|
|
Vector3 zv = Vector3(0, 0, Extents.Z * 2);
|
|
|
|
Vector3::TransformNormal(xv, Transformation, xv);
|
|
Vector3::TransformNormal(yv, Transformation, yv);
|
|
Vector3::TransformNormal(zv, Transformation, zv);
|
|
|
|
return Vector3(xv.LengthSquared(), yv.LengthSquared(), zv.LengthSquared());
|
|
}
|
|
|
|
BoundingBox OrientedBoundingBox::GetBoundingBox() const
|
|
{
|
|
BoundingBox result;
|
|
|
|
Vector3 corners[8];
|
|
GetCorners(corners);
|
|
|
|
BoundingBox::FromPoints(corners, 8, result);
|
|
|
|
return result;
|
|
}
|
|
|
|
void OrientedBoundingBox::GetBoundingBox(BoundingBox& result) const
|
|
{
|
|
Vector3 corners[8];
|
|
GetCorners(corners);
|
|
|
|
BoundingBox::FromPoints(corners, 8, result);
|
|
}
|
|
|
|
ContainmentType OrientedBoundingBox::Contains(const Vector3& point, float* distance) const
|
|
{
|
|
// Transform the point into the obb coordinates
|
|
Matrix invTrans;
|
|
Matrix::Invert(Transformation, invTrans);
|
|
|
|
Vector3 locPoint;
|
|
Vector3::TransformCoordinate(point, invTrans, locPoint);
|
|
|
|
locPoint.X = Math::Abs(locPoint.X);
|
|
locPoint.Y = Math::Abs(locPoint.Y);
|
|
locPoint.Z = Math::Abs(locPoint.Z);
|
|
|
|
if (distance)
|
|
{
|
|
// Get minimum distance to edge in local space
|
|
Vector3 tmp;
|
|
Vector3::Subtract(Extents, locPoint, tmp);
|
|
tmp.Absolute();
|
|
const float minDstToEdgeLocal = tmp.MinValue();
|
|
|
|
// Transform distance to world space
|
|
Vector3 dstVec = Vector3::UnitX * minDstToEdgeLocal;
|
|
Vector3::TransformNormal(dstVec, Transformation, dstVec);
|
|
*distance = dstVec.Length();
|
|
}
|
|
|
|
// Simple axes-aligned BB check
|
|
if (locPoint.X < Extents.X && locPoint.Y < Extents.Y && locPoint.Z < Extents.Z)
|
|
return ContainmentType::Contains;
|
|
if (Vector3::NearEqual(locPoint, Extents))
|
|
return ContainmentType::Intersects;
|
|
return ContainmentType::Disjoint;
|
|
}
|
|
|
|
ContainmentType OrientedBoundingBox::Contains(int32 pointsCnt, Vector3 points[]) const
|
|
{
|
|
ASSERT(pointsCnt > 0);
|
|
|
|
Matrix invTrans;
|
|
Matrix::Invert(Transformation, invTrans);
|
|
|
|
bool containsAll = true;
|
|
bool containsAny = false;
|
|
|
|
for (int32 i = 0; i < pointsCnt; i++)
|
|
{
|
|
Vector3 locPoint;
|
|
Vector3::TransformCoordinate(points[i], invTrans, locPoint);
|
|
|
|
locPoint.X = Math::Abs(locPoint.X);
|
|
locPoint.Y = Math::Abs(locPoint.Y);
|
|
locPoint.Z = Math::Abs(locPoint.Z);
|
|
|
|
// Simple axes-aligned BB check
|
|
if (Vector3::NearEqual(locPoint, Extents))
|
|
containsAny = true;
|
|
if (locPoint.X < Extents.X && locPoint.Y < Extents.Y && locPoint.Z < Extents.Z)
|
|
containsAny = true;
|
|
else
|
|
containsAll = false;
|
|
}
|
|
|
|
if (containsAll)
|
|
return ContainmentType::Contains;
|
|
if (containsAny)
|
|
return ContainmentType::Intersects;
|
|
return ContainmentType::Disjoint;
|
|
}
|
|
|
|
ContainmentType OrientedBoundingBox::Contains(const BoundingSphere& sphere, bool ignoreScale) const
|
|
{
|
|
Matrix invTrans;
|
|
Matrix::Invert(Transformation, invTrans);
|
|
|
|
// Transform sphere center into the obb coordinates
|
|
Vector3 locCenter;
|
|
Vector3::TransformCoordinate(sphere.Center, invTrans, locCenter);
|
|
|
|
float locRadius;
|
|
if (ignoreScale)
|
|
{
|
|
locRadius = sphere.Radius;
|
|
}
|
|
else
|
|
{
|
|
// Transform sphere radius into the obb coordinates
|
|
Vector3 vRadius = Vector3::UnitX * sphere.Radius;
|
|
Vector3::TransformNormal(vRadius, invTrans, vRadius);
|
|
locRadius = vRadius.Length();
|
|
}
|
|
|
|
// Perform regular BoundingBox to BoundingSphere containment check
|
|
const Vector3 minusExtents = -Extents;
|
|
Vector3 vector;
|
|
Vector3::Clamp(locCenter, minusExtents, Extents, vector);
|
|
const float distance = Vector3::DistanceSquared(locCenter, vector);
|
|
|
|
if (distance > locRadius * locRadius)
|
|
return ContainmentType::Disjoint;
|
|
|
|
if (minusExtents.X + locRadius <= locCenter.X
|
|
&& locCenter.X <= Extents.X - locRadius
|
|
&& (Extents.X - minusExtents.X > locRadius
|
|
&& minusExtents.Y + locRadius <= locCenter.Y)
|
|
&& (locCenter.Y <= Extents.Y - locRadius
|
|
&& Extents.Y - minusExtents.Y > locRadius
|
|
&& (minusExtents.Z + locRadius <= locCenter.Z
|
|
&& locCenter.Z <= Extents.Z - locRadius
|
|
&& Extents.Z - minusExtents.Z > locRadius)))
|
|
{
|
|
return ContainmentType::Contains;
|
|
}
|
|
|
|
return ContainmentType::Intersects;
|
|
}
|
|
|
|
/*ContainmentType OrientedBoundingBox::Contains(const OrientedBoundingBox& obb)
|
|
{
|
|
Vector3 obbCorners[8];
|
|
obb.GetCorners(obbCorners);
|
|
auto cornersCheck = Contains(8, obbCorners);
|
|
if (cornersCheck != ContainmentType::Disjoint)
|
|
return cornersCheck;
|
|
|
|
// http://www.3dkingdoms.com/weekly/bbox.cpp
|
|
Vector3 SizeA = Extents;
|
|
Vector3 SizeB = obb.Extents;
|
|
Vector3 RotA[3];
|
|
Vector3 RotB[3];
|
|
GetRows(Transformation, RotA);
|
|
GetRows(obb.Transformation, RotB);
|
|
|
|
Matrix R = Matrix::Identity; // Rotation from B to A
|
|
Matrix AR = Matrix::Identity; // absolute values of R matrix, to use with box extents
|
|
|
|
float ExtentA, ExtentB, Separation;
|
|
int i, k;
|
|
|
|
// Calculate B to A rotation matrix
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
for (k = 0; k < 3; k++)
|
|
{
|
|
R[i, k] = Vector3::Dot(RotA[i], RotB[k]);
|
|
AR[i, k] = Math::Abs(R[i, k]);
|
|
}
|
|
}
|
|
|
|
// Vector separating the centers of Box B and of Box A
|
|
Vector3 vSepWS = obb.GetCenter() - GetCenter();
|
|
|
|
// Rotated into Box A's coordinates
|
|
Vector3 vSepA = Vector3(Vector3::Dot(vSepWS, RotA[0]), Vector3::Dot(vSepWS, RotA[1]), Vector3::Dot(vSepWS, RotA[2]));
|
|
|
|
// Test if any of A's basis vectors separate the box
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
ExtentA = SizeA[i];
|
|
ExtentB = Vector3::Dot(SizeB, Vector3(AR[i, 0], AR[i, 1], AR[i, 2]));
|
|
Separation = Math::Abs(vSepA[i]);
|
|
|
|
if (Separation > ExtentA + ExtentB)
|
|
return ContainmentType::Disjoint;
|
|
}
|
|
|
|
// Test if any of B's basis vectors separate the box
|
|
for (k = 0; k < 3; k++)
|
|
{
|
|
ExtentA = Vector3::Dot(SizeA, Vector3(AR[0, k], AR[1, k], AR[2, k]));
|
|
ExtentB = SizeB[k];
|
|
Separation = Math::Abs(Vector3::Dot(vSepA, Vector3(R[0, k], R[1, k], R[2, k])));
|
|
|
|
if (Separation > ExtentA + ExtentB)
|
|
return ContainmentType::Disjoint;
|
|
}
|
|
|
|
// Now test Cross Products of each basis vector combination ( A[i], B[k] )
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
for (k = 0; k < 3; k++)
|
|
{
|
|
int i1 = (i + 1) % 3, i2 = (i + 2) % 3;
|
|
int k1 = (k + 1) % 3, k2 = (k + 2) % 3;
|
|
ExtentA = SizeA[i1] * AR[i2, k] + SizeA[i2] * AR[i1, k];
|
|
ExtentB = SizeB[k1] * AR[i, k2] + SizeB[k2] * AR[i, k1];
|
|
Separation = Math::Abs(vSepA[i2] * R[i1, k] - vSepA[i1] * R[i2, k]);
|
|
if (Separation > ExtentA + ExtentB)
|
|
return ContainmentType::Disjoint;
|
|
}
|
|
}
|
|
|
|
// No separating axis found, the boxes overlap
|
|
return ContainmentType::Intersects;
|
|
}*/
|
|
|
|
bool OrientedBoundingBox::Intersects(const Ray& ray, Vector3& point) const
|
|
{
|
|
// Put ray in box space
|
|
Matrix invTrans;
|
|
Matrix::Invert(Transformation, invTrans);
|
|
|
|
Ray bRay;
|
|
Vector3::TransformNormal(ray.Direction, invTrans, bRay.Direction);
|
|
Vector3::TransformCoordinate(ray.Position, invTrans, bRay.Position);
|
|
|
|
// Perform a regular ray to BoundingBox check
|
|
const BoundingBox bb = BoundingBox(-Extents, Extents);
|
|
const bool intersects = CollisionsHelper::RayIntersectsBox(bRay, bb, point);
|
|
|
|
// Put the result intersection back to world
|
|
if (intersects)
|
|
Vector3::TransformCoordinate(point, Transformation, point);
|
|
|
|
return intersects;
|
|
}
|
|
|
|
bool OrientedBoundingBox::Intersects(const Ray& ray, float& distance) const
|
|
{
|
|
Vector3 point;
|
|
const bool result = Intersects(ray, point);
|
|
distance = Vector3::Distance(ray.Position, point);
|
|
return result;
|
|
}
|
|
|
|
bool OrientedBoundingBox::Intersects(const Ray& ray, float& distance, Vector3& normal) const
|
|
{
|
|
// Put ray in box space
|
|
Matrix invTrans;
|
|
Matrix::Invert(Transformation, invTrans);
|
|
|
|
Ray bRay;
|
|
Vector3::TransformNormal(ray.Direction, invTrans, bRay.Direction);
|
|
Vector3::TransformCoordinate(ray.Position, invTrans, bRay.Position);
|
|
|
|
// Perform a regular ray to BoundingBox check
|
|
const BoundingBox bb(-Extents, Extents);
|
|
if (CollisionsHelper::RayIntersectsBox(bRay, bb, distance, normal))
|
|
{
|
|
// Put the result intersection back to world
|
|
Vector3::TransformNormal(normal, Transformation, normal);
|
|
normal.Normalize();
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|