1
0
mirror of https://github.com/borgesdan/xn65 synced 2024-12-29 21:54:47 +01:00
xn65/framework/common/collision.cpp
2024-06-03 21:55:09 -03:00

575 lines
15 KiB
C++

#include "xna/common/collision.hpp"
namespace xna {
Plane::Plane(Vector3 const& point1, Vector3 const& point2, Vector3 const& point3) {
const auto num1 = point2.X - point1.X;
const auto num2 = point2.Y - point1.Y;
const auto num3 = point2.Z - point1.Z;
const auto num4 = point3.X - point1.X;
const auto num5 = point3.Y - point1.Y;
const auto num6 = point3.Z - point1.Z;
const auto num7 = (num2 * num6 - num3 * num5);
const auto num8 = (num3 * num4 - num1 * num6);
const auto num9 = (num1 * num5 - num2 * num4);
const auto num10 = 1.0f / std::sqrt(num7 * num7 + num8 * num8 + num9 * num9);
Normal.X = num7 * num10;
Normal.Y = num8 * num10;
Normal.Z = num9 * num10;
D = -(Normal.X * point1.X + Normal.Y * point1.Y + Normal.Z * point1.Z);
}
void Plane::Normalize() {
const auto d = (Normal.X * Normal.X + Normal.Y * Normal.Y + Normal.Z * Normal.Z);
if (std::abs(d - 1.0f) < 1.1920928955078125E-07)
return;
const auto num = 1.0f / std::sqrt(d);
Normal.X *= num;
Normal.Y *= num;
Normal.Z *= num;
D *= num;
}
Plane Plane::Normalize(Plane const& value) {
auto p = value;
p.Normalize();
return p;
}
std::optional<float> Plane::Intersects(Ray const& ray) const {
const auto num1 = (Normal.X * ray.Direction.X + Normal.Y * ray.Direction.Y + Normal.Z * ray.Direction.Z);
if (std::abs(num1) < 9.9999997473787516E-06)
return {};
const auto num2 = (Normal.X * ray.Position.X + Normal.Y * ray.Position.Y + Normal.Z * ray.Position.Z);
auto num3 = (-D - num2) / num1;
if (num3 < 0.0)
{
if (num3 < -9.9999997473787516E-06)
return {};
num3 = 0.0f;
}
return num3;
}
bool BoundingFrustum::Intersects(BoundingBox const& box) {
gjk.Reset();
Vector3 result1 = Vector3::Subtract(corners[0], box.Min);
if (result1.LengthSquared() < 9.9999997473787516E-06)
result1 = Vector3::Subtract(corners[0], box.Max);
auto num1 = FloatMaxValue;
float num2 = 0;
do
{
Vector3 v;
v.X = -result1.X;
v.Y = -result1.Y;
v.Z = -result1.Z;
Vector3 result2;
SupportMapping(v, result2);
Vector3 result3;
box.SupportMapping(result1, result3);
Vector3 result4 = Vector3::Subtract(result2, result3);
if (result1.X * result4.X + result1.Y * result4.Y + result1.Z * result4.Z > 0.0)
return false;
gjk.AddSupportPoint(result4);
result1 = gjk.ClosestPoint();
float num3 = num1;
num1 = result1.LengthSquared();
if (num3 - num1 <= 9.9999997473787516E-06 * num3)
return false;
num2 = 4E-05f * gjk.MaxLengthSquared();
} while (!gjk.FullSimplex() && num1 >= num2);
return true;
}
std::optional<float> BoundingFrustum::Intersects(Ray const& ray) const {
auto result1 = Contains(ray.Position);
if (result1 == ContainmentType::Contains)
return 0.0F;
auto num1 = FloatMinValue;
auto num2 = FloatMaxValue;
for (size_t i = 0; i < planes.size(); ++i) {
const auto& plane = planes[i];
Vector3 normal = plane.Normal;
const auto result2 = Vector3::Dot(ray.Direction, normal);
float result3 = Vector3::Dot(ray.Position, normal);
result3 += plane.D;
if (std::abs(static_cast<double>(result2)) < 9.9999997473787516E-06)
{
if (result3 > 0.0)
return {};
}
else
{
const auto num3 = -result3 / result2;
if (result2 < 0.0)
{
if (num3 > num2)
return {};
if (num3 > num1)
num1 = num3;
}
else
{
if (num3 < num1)
return {};
if (num3 < num2)
num2 = num3;
}
}
}
auto num4 = num1 >= 0.0F ? num1 : num2;
if (num4 < 0.0)
return{};
return num4;
}
bool BoundingFrustum::Intersects(BoundingSphere const& sphere) {
gjk.Reset();
auto result1 = Vector3::Subtract(corners[0], sphere.Center);
if (result1.LengthSquared() < 9.9999997473787516E-06)
result1 = Vector3::UnitX();
auto num1 = FloatMaxValue;
auto num2 = 0.0F;
do
{
Vector3 v;
v.X = -result1.X;
v.Y = -result1.Y;
v.Z = -result1.Z;
Vector3 result2;
SupportMapping(v, result2);
Vector3 result3;
sphere.SupportMapping(result1, result3);
Vector3 result4 = Vector3::Subtract(result2, result3);
if (result1.X * result4.X + result1.Y * result4.Y + result1.Z * result4.Z > 0.0)
return false;
gjk.AddSupportPoint(result4);
result1 = gjk.ClosestPoint();
auto num3 = num1;
num1 = result1.LengthSquared();
if (num3 - num1 <= 9.9999997473787516E-06 * num3)
return false;
num2 = 4E-05f * gjk.MaxLengthSquared();
} while (!gjk.FullSimplex() && num1 >= num2);
return true;
}
bool BoundingFrustum::Intersects(BoundingFrustum const& frustum) {
gjk.Reset();
Vector3 result1 = Vector3::Subtract(corners[0], frustum.corners[0]);
if (result1.LengthSquared() < 9.9999997473787516E-06)
result1 = Vector3::Subtract(corners[0], frustum.corners[1]);
float num1 = FloatMaxValue;
float num2 = 0;
do
{
Vector3 v;
v.X = -result1.X;
v.Y = -result1.Y;
v.Z = -result1.Z;
Vector3 result2;
SupportMapping(v, result2);
Vector3 result3;
frustum.SupportMapping(result1, result3);
const auto result4 = Vector3::Subtract(result2, result3);
if (result1.X * result4.X + result1.Y * result4.Y + result1.Z * result4.Z > 0.0)
return false;
gjk.AddSupportPoint(result4);
result1 = gjk.ClosestPoint();
const auto num3 = num1;
num1 = result1.LengthSquared();
num2 = 4E-05f * gjk.MaxLengthSquared();
if (num3 - num1 <= 9.9999997473787516E-06 * num3)
return false;
} while (!gjk.FullSimplex() && num1 >= num2);
return true;
}
ContainmentType BoundingFrustum::Contains(BoundingFrustum const& frustum) {
ContainmentType containmentType = ContainmentType::Disjoint;
if (Intersects(frustum)) {
containmentType = ContainmentType::Contains;
for (size_t index = 0; index < corners.size(); ++index) {
if (Contains(frustum.corners[index]) == ContainmentType::Disjoint)
{
containmentType = ContainmentType::Intersects;
break;
}
}
}
return containmentType;
}
std::optional<float> BoundingBox::Intersects(Ray const& ray) const {
float num1 = 0.0f;
float num2 = FloatMaxValue;
if (std::abs(ray.Direction.X) < 9.9999999747524271E-07)
{
if (ray.Position.X < Min.X || ray.Position.X > Max.X)
return {};
}
else
{
const auto num3 = 1.0f / ray.Direction.X;
auto num4 = (Min.X - ray.Position.X) * num3;
auto num5 = (Max.X - ray.Position.X) * num3;
if (num4 > num5)
{
float num6 = num4;
num4 = num5;
num5 = num6;
}
num1 = MathHelper::Max(num4, num1);
num2 = MathHelper::Min(num5, num2);
if (num1 > num2)
return {};
}
if (std::abs(ray.Direction.Y) < 9.9999999747524271E-07)
{
if (ray.Position.Y < Min.Y || ray.Position.Y > Max.Y)
return {};
}
else
{
const auto num7 = 1.0f / ray.Direction.Y;
auto num8 = (Min.Y - ray.Position.Y) * num7;
auto num9 = (Max.Y - ray.Position.Y) * num7;
if (num8 > num9)
{
float num10 = num8;
num8 = num9;
num9 = num10;
}
num1 = MathHelper::Max(num8, num1);
num2 = MathHelper::Min(num9, num2);
if (num1 > num2)
return {};
}
if (std::abs(ray.Direction.Z) < 9.9999999747524271E-07)
{
if (ray.Position.Z < Min.Z || ray.Position.Z > Max.Z)
return {};
}
else
{
const auto num11 = 1.0f / ray.Direction.Z;
auto num12 = (Min.Z - ray.Position.Z) * num11;
auto num13 = (Max.Z - ray.Position.Z) * num11;
if (num12 > num13)
{
float num14 = num12;
num12 = num13;
num13 = num14;
}
num1 = MathHelper::Max(num12, num1);
const auto num15 = MathHelper::Min(num13, num2);
if (num1 > num15)
return {};
}
return num1;
}
ContainmentType BoundingBox::Contains(BoundingFrustum& frustum) const {
if (!frustum.Intersects(*this))
return ContainmentType::Disjoint;
for (size_t i = 0; i < frustum.CornerCount; ++i) {
const auto corner = frustum[i];
if (Contains(corner) == ContainmentType::Disjoint)
return ContainmentType::Intersects;
}
return ContainmentType::Contains;
}
BoundingSphere BoundingSphere::CreateFromBoundingBox(BoundingBox const& box) {
BoundingSphere fromBoundingBox;
fromBoundingBox.Center = Vector3::Lerp(box.Min, box.Max, 0.5f);
const auto result = Vector3::Distance(box.Min, box.Max);
fromBoundingBox.Radius = result * 0.5f;
return fromBoundingBox;
}
BoundingSphere BoundingSphere::CreateFromFrustum(BoundingFrustum const& frustum) {
return BoundingSphere::CreateFromPoints(frustum.corners);
}
BoundingSphere BoundingSphere::CreateFromPoints(std::vector<Vector3> const& points) {
Vector3 current;
auto vector3_1 = current = points[0];
auto vector3_2 = current;
auto vector3_3 = current;
auto vector3_4 = current;
auto vector3_5 = current;
auto vector3_6 = current;
for (size_t i = 0; i < points.size(); ++i) {
const auto& point = points[i];
if (point.X < vector3_6.X)
vector3_6 = point;
if (point.X > vector3_5.X)
vector3_5 = point;
if (point.Y < vector3_4.Y)
vector3_4 = point;
if (point.Y > vector3_3.Y)
vector3_3 = point;
if (point.Z < vector3_2.Z)
vector3_2 = point;
if (point.Z > vector3_1.Z)
vector3_1 = point;
}
auto result1 = Vector3::Distance(vector3_5, vector3_6);
auto result2 = Vector3::Distance(vector3_3, vector3_4);
auto result3 = Vector3::Distance(vector3_1, vector3_2);
Vector3 result4;
float num1 = 0;
if (result1 > result2)
{
if (result1 > result3)
{
result4 = Vector3::Lerp(vector3_5, vector3_6, 0.5f);
num1 = result1 * 0.5f;
}
else
{
result4 = Vector3::Lerp(vector3_1, vector3_2, 0.5f);
num1 = result3 * 0.5f;
}
}
else if (result2 > result3)
{
result4 = Vector3::Lerp(vector3_3, vector3_4, 0.5f);
num1 = result2 * 0.5f;
}
else
{
result4 = Vector3::Lerp(vector3_1, vector3_2, 0.5f);
num1 = result3 * 0.5f;
}
for (size_t i = 0; i < points.size(); ++i) {
const auto& point = points[i];
Vector3 vector3_7;
vector3_7.X = point.X - result4.X;
vector3_7.Y = point.Y - result4.Y;
vector3_7.Z = point.Z - result4.Z;
float num2 = vector3_7.Length();
if (num2 > num1) {
num1 = (num1 + num2) * 0.5F;
result4 += (1.0F - num1 / num2) * vector3_7;
}
}
BoundingSphere fromPoints;
fromPoints.Center = result4;
fromPoints.Radius = num1;
return fromPoints;
}
ContainmentType BoundingSphere::Contains(BoundingFrustum& frustum) const {
if (!frustum.Intersects(*this))
return ContainmentType::Disjoint;
float num = Radius * Radius;
for (size_t i = 0; i < BoundingFrustum::CornerCount; ++i)
{
const auto corner = frustum[i];
Vector3 vector3;
vector3.X = corner.X - Center.X;
vector3.Y = corner.Y - Center.Y;
vector3.Z = corner.Z - Center.Z;
if (vector3.LengthSquared() > num)
return ContainmentType::Intersects;
}
return ContainmentType::Contains;
}
ContainmentType BoundingSphere::Contains(BoundingSphere const& sphere) const {
const auto result = Vector3::Distance(Center, sphere.Center);
const auto radius1 = Radius;
const auto radius2 = sphere.Radius;
if (radius1 + radius2 < result)
return ContainmentType::Disjoint;
return radius1 - radius2 < result
? ContainmentType::Intersects
: ContainmentType::Contains;
}
void BoundingSphere::SupportMapping(Vector3 const& v, Vector3& result) const {
const auto num = Radius / v.Length();
result.X = Center.X + v.X * num;
result.Y = Center.Y + v.Y * num;
result.Z = Center.Z + v.Z * num;
}
BoundingSphere BoundingSphere::Transform(Matrix const& matrix) const {
BoundingSphere boundingSphere;
boundingSphere.Center = Vector3::Transform(Center, matrix);
float d = MathHelper::Max((matrix.M11 * matrix.M11 + matrix.M12 * matrix.M12 + matrix.M13 * matrix.M13), MathHelper::Max((matrix.M21 * matrix.M21 + matrix.M22 * matrix.M22 + matrix.M23 * matrix.M23), (matrix.M31 * matrix.M31 + matrix.M32 * matrix.M32 + matrix.M33 * matrix.M33)));
boundingSphere.Radius = Radius * std::sqrt(d);
return boundingSphere;
}
ContainmentType BoundingSphere::Contains(BoundingBox const& box) const {
if (!box.Intersects(*this))
return ContainmentType::Disjoint;
float num = Radius * Radius;
Vector3 vector3;
vector3.X = Center.X - box.Min.X;
vector3.Y = Center.Y - box.Max.Y;
vector3.Z = Center.Z - box.Max.Z;
if (vector3.LengthSquared() > num)
return ContainmentType::Intersects;
vector3.X = Center.X - box.Max.X;
vector3.Y = Center.Y - box.Max.Y;
vector3.Z = Center.Z - box.Max.Z;
if (vector3.LengthSquared() > num)
return ContainmentType::Intersects;
vector3.X = Center.X - box.Max.X;
vector3.Y = Center.Y - box.Min.Y;
vector3.Z = Center.Z - box.Max.Z;
if (vector3.LengthSquared() > num)
return ContainmentType::Intersects;
vector3.X = Center.X - box.Min.X;
vector3.Y = Center.Y - box.Min.Y;
vector3.Z = Center.Z - box.Max.Z;
if (vector3.LengthSquared() > num)
return ContainmentType::Intersects;
vector3.X = Center.X - box.Min.X;
vector3.Y = Center.Y - box.Max.Y;
vector3.Z = Center.Z - box.Min.Z;
if (vector3.LengthSquared() > num)
return ContainmentType::Intersects;
vector3.X = Center.X - box.Max.X;
vector3.Y = Center.Y - box.Max.Y;
vector3.Z = Center.Z - box.Min.Z;
if (vector3.LengthSquared() > num)
return ContainmentType::Intersects;
vector3.X = Center.X - box.Max.X;
vector3.Y = Center.Y - box.Min.Y;
vector3.Z = Center.Z - box.Min.Z;
if (vector3.LengthSquared() > num)
return ContainmentType::Intersects;
vector3.X = Center.X - box.Min.X;
vector3.Y = Center.Y - box.Min.Y;
vector3.Z = Center.Z - box.Min.Z;
return vector3.LengthSquared() > num
? ContainmentType::Intersects
: ContainmentType::Contains;
}
ContainmentType BoundingSphere::Contains(Vector3 const& point) const {
return Vector3::DistanceSquared(point, Center) >= Radius * Radius
? ContainmentType::Disjoint
: ContainmentType::Contains;
}
std::optional<float> BoundingSphere::Intersects(Ray const& ray) const {
const auto num1 = Center.X - ray.Position.X;
const auto num2 = Center.Y - ray.Position.Y;
const auto num3 = Center.Z - ray.Position.Z;
const auto num4 = (num1 * num1 + num2 * num2 + num3 * num3);
const auto num5 = Radius * Radius;
if (num4 <= num5)
return 0.0f;
const auto num6 = (num1 * ray.Direction.X + num2 * ray.Direction.Y + num3 * ray.Direction.Z);
if (num6 < 0.0)
return {};
const auto num7 = num4 - num6 * num6;
if (num7 > num5)
return {};
const auto num8 = std::sqrt(num5 - num7);
return num6 - num8;
}
BoundingSphere BoundingSphere::CreateMerged(BoundingSphere const& original, BoundingSphere const& additional) {
Vector3 result = Vector3::Subtract(additional.Center, original.Center);
float num1 = result.Length();
float radius1 = original.Radius;
float radius2 = additional.Radius;
if (radius1 + radius2 >= num1) {
if (radius1 - radius2 >= num1)
return original;
if (radius2 - radius1 >= num1)
return additional;
}
Vector3 vector3 = result * (1.0f / num1);
const auto num2 = MathHelper::Min(-radius1, num1 - radius2);
const auto num3 = (MathHelper::Max(radius1, num1 + radius2) - num2) * 0.5F;
BoundingSphere merged;
merged.Center = original.Center + vector3 * (num3 + num2);
merged.Radius = num3;
return merged;
}
}