Turánszki János
965 lines
27 KiB
C++
965 lines
27 KiB
C++
#include "wiPrimitive.h"
|
|
|
|
namespace wi::primitive
|
|
{
|
|
|
|
void AABB::createFromHalfWidth(const XMFLOAT3& center, const XMFLOAT3& halfwidth)
|
|
{
|
|
_min = XMFLOAT3(center.x - halfwidth.x, center.y - halfwidth.y, center.z - halfwidth.z);
|
|
_max = XMFLOAT3(center.x + halfwidth.x, center.y + halfwidth.y, center.z + halfwidth.z);
|
|
}
|
|
AABB AABB::transform(const XMMATRIX& mat) const
|
|
{
|
|
const XMVECTOR vcorners[8] = {
|
|
XMVector3Transform(XMLoadFloat3(&_min), mat),
|
|
XMVector3Transform(XMVectorSet(_min.x, _max.y, _min.z, 1), mat),
|
|
XMVector3Transform(XMVectorSet(_min.x, _max.y, _max.z, 1), mat),
|
|
XMVector3Transform(XMVectorSet(_min.x, _min.y, _max.z, 1), mat),
|
|
XMVector3Transform(XMVectorSet(_max.x, _min.y, _min.z, 1), mat),
|
|
XMVector3Transform(XMVectorSet(_max.x, _max.y, _min.z, 1), mat),
|
|
XMVector3Transform(XMLoadFloat3(&_max), mat),
|
|
XMVector3Transform(XMVectorSet(_max.x, _min.y, _max.z, 1), mat),
|
|
};
|
|
XMVECTOR vmin = vcorners[0];
|
|
XMVECTOR vmax = vcorners[0];
|
|
vmin = XMVectorMin(vmin, vcorners[1]);
|
|
vmax = XMVectorMax(vmax, vcorners[1]);
|
|
vmin = XMVectorMin(vmin, vcorners[2]);
|
|
vmax = XMVectorMax(vmax, vcorners[2]);
|
|
vmin = XMVectorMin(vmin, vcorners[3]);
|
|
vmax = XMVectorMax(vmax, vcorners[3]);
|
|
vmin = XMVectorMin(vmin, vcorners[4]);
|
|
vmax = XMVectorMax(vmax, vcorners[4]);
|
|
vmin = XMVectorMin(vmin, vcorners[5]);
|
|
vmax = XMVectorMax(vmax, vcorners[5]);
|
|
vmin = XMVectorMin(vmin, vcorners[6]);
|
|
vmax = XMVectorMax(vmax, vcorners[6]);
|
|
vmin = XMVectorMin(vmin, vcorners[7]);
|
|
vmax = XMVectorMax(vmax, vcorners[7]);
|
|
|
|
XMFLOAT3 min, max;
|
|
XMStoreFloat3(&min, vmin);
|
|
XMStoreFloat3(&max, vmax);
|
|
return AABB(min, max);
|
|
}
|
|
AABB AABB::transform(const XMFLOAT4X4& mat) const
|
|
{
|
|
return transform(XMLoadFloat4x4(&mat));
|
|
}
|
|
XMFLOAT3 AABB::getCenter() const
|
|
{
|
|
return XMFLOAT3((_min.x + _max.x) * 0.5f, (_min.y + _max.y) * 0.5f, (_min.z + _max.z) * 0.5f);
|
|
}
|
|
XMFLOAT3 AABB::getHalfWidth() const
|
|
{
|
|
XMFLOAT3 center = getCenter();
|
|
return XMFLOAT3(abs(_max.x - center.x), abs(_max.y - center.y), abs(_max.z - center.z));
|
|
}
|
|
XMMATRIX AABB::AABB::getAsBoxMatrix() const
|
|
{
|
|
XMFLOAT3 ext = getHalfWidth();
|
|
XMMATRIX sca = XMMatrixScaling(ext.x, ext.y, ext.z);
|
|
XMFLOAT3 pos = getCenter();
|
|
XMMATRIX tra = XMMatrixTranslation(pos.x, pos.y, pos.z);
|
|
|
|
return sca * tra;
|
|
}
|
|
XMMATRIX AABB::AABB::getUnormRemapMatrix() const
|
|
{
|
|
return
|
|
XMMatrixScaling(_max.x - _min.x, _max.y - _min.y, _max.z - _min.z) *
|
|
XMMatrixTranslation(_min.x, _min.y, _min.z)
|
|
;
|
|
}
|
|
float AABB::getArea() const
|
|
{
|
|
XMFLOAT3 _min = getMin();
|
|
XMFLOAT3 _max = getMax();
|
|
return (_max.x - _min.x) * (_max.y - _min.y) * (_max.z - _min.z);
|
|
}
|
|
float AABB::getRadius() const
|
|
{
|
|
XMFLOAT3 abc = getHalfWidth();
|
|
return std::sqrt(sqr(std::sqrt(sqr(abc.x) + sqr(abc.y))) + sqr(abc.z));
|
|
}
|
|
AABB::INTERSECTION_TYPE AABB::intersects(const AABB& b) const
|
|
{
|
|
if (!IsValid() || !b.IsValid())
|
|
return OUTSIDE;
|
|
|
|
XMFLOAT3 aMin = getMin(), aMax = getMax();
|
|
XMFLOAT3 bMin = b.getMin(), bMax = b.getMax();
|
|
|
|
if (bMin.x >= aMin.x && bMax.x <= aMax.x &&
|
|
bMin.y >= aMin.y && bMax.y <= aMax.y &&
|
|
bMin.z >= aMin.z && bMax.z <= aMax.z)
|
|
{
|
|
return INSIDE;
|
|
}
|
|
|
|
if (aMax.x < bMin.x || aMin.x > bMax.x)
|
|
return OUTSIDE;
|
|
if (aMax.y < bMin.y || aMin.y > bMax.y)
|
|
return OUTSIDE;
|
|
if (aMax.z < bMin.z || aMin.z > bMax.z)
|
|
return OUTSIDE;
|
|
|
|
return INTERSECTS;
|
|
}
|
|
AABB::INTERSECTION_TYPE AABB::intersects2D(const AABB& b) const
|
|
{
|
|
if (!IsValid() || !b.IsValid())
|
|
return OUTSIDE;
|
|
|
|
XMFLOAT3 aMin = getMin(), aMax = getMax();
|
|
XMFLOAT3 bMin = b.getMin(), bMax = b.getMax();
|
|
|
|
if (bMin.x >= aMin.x && bMax.x <= aMax.x &&
|
|
bMin.y >= aMin.y && bMax.y <= aMax.y)
|
|
{
|
|
return INSIDE;
|
|
}
|
|
|
|
if (aMax.x < bMin.x || aMin.x > bMax.x)
|
|
return OUTSIDE;
|
|
if (aMax.y < bMin.y || aMin.y > bMax.y)
|
|
return OUTSIDE;
|
|
|
|
return INTERSECTS;
|
|
}
|
|
bool AABB::intersects(const XMFLOAT3& p) const
|
|
{
|
|
if (!IsValid())
|
|
return false;
|
|
if (p.x > _max.x) return false;
|
|
if (p.x < _min.x) return false;
|
|
if (p.y > _max.y) return false;
|
|
if (p.y < _min.y) return false;
|
|
if (p.z > _max.z) return false;
|
|
if (p.z < _min.z) return false;
|
|
return true;
|
|
}
|
|
bool AABB::intersects(const XMVECTOR& P) const
|
|
{
|
|
XMFLOAT3 p;
|
|
XMStoreFloat3(&p, P);
|
|
return intersects(p);
|
|
}
|
|
bool AABB::intersects(const Ray& ray) const
|
|
{
|
|
if (!IsValid())
|
|
return false;
|
|
if (intersects(ray.origin))
|
|
return true;
|
|
|
|
XMFLOAT3 MIN = getMin();
|
|
XMFLOAT3 MAX = getMax();
|
|
|
|
float tx1 = (MIN.x - ray.origin.x) * ray.direction_inverse.x;
|
|
float tx2 = (MAX.x - ray.origin.x) * ray.direction_inverse.x;
|
|
|
|
float tmin = std::min(tx1, tx2);
|
|
float tmax = std::max(tx1, tx2);
|
|
if (ray.TMax < tmin || ray.TMin > tmax)
|
|
return false;
|
|
|
|
float ty1 = (MIN.y - ray.origin.y) * ray.direction_inverse.y;
|
|
float ty2 = (MAX.y - ray.origin.y) * ray.direction_inverse.y;
|
|
|
|
tmin = std::max(tmin, std::min(ty1, ty2));
|
|
tmax = std::min(tmax, std::max(ty1, ty2));
|
|
if (ray.TMax < tmin || ray.TMin > tmax)
|
|
return false;
|
|
|
|
float tz1 = (MIN.z - ray.origin.z) * ray.direction_inverse.z;
|
|
float tz2 = (MAX.z - ray.origin.z) * ray.direction_inverse.z;
|
|
|
|
tmin = std::max(tmin, std::min(tz1, tz2));
|
|
tmax = std::min(tmax, std::max(tz1, tz2));
|
|
if (ray.TMax < tmin || ray.TMin > tmax)
|
|
return false;
|
|
|
|
return tmax >= tmin;
|
|
}
|
|
bool AABB::intersects(const Sphere& sphere) const
|
|
{
|
|
if (!IsValid())
|
|
return false;
|
|
return sphere.intersects(*this);
|
|
}
|
|
bool AABB::intersects(const BoundingFrustum& frustum) const
|
|
{
|
|
if (!IsValid())
|
|
return false;
|
|
BoundingBox bb = BoundingBox(getCenter(), getHalfWidth());
|
|
bool intersection = frustum.Intersects(bb);
|
|
return intersection;
|
|
}
|
|
bool AABB::intersects(const BoundingBox& other) const
|
|
{
|
|
BoundingBox bb = BoundingBox(getCenter(), getHalfWidth());
|
|
return bb.Intersects(other);
|
|
}
|
|
bool AABB::intersects(const BoundingOrientedBox& other) const
|
|
{
|
|
BoundingBox bb = BoundingBox(getCenter(), getHalfWidth());
|
|
return bb.Intersects(other);
|
|
}
|
|
AABB AABB::operator* (float a)
|
|
{
|
|
XMFLOAT3 min = getMin();
|
|
XMFLOAT3 max = getMax();
|
|
min.x *= a;
|
|
min.y *= a;
|
|
min.z *= a;
|
|
max.x *= a;
|
|
max.y *= a;
|
|
max.z *= a;
|
|
return AABB(min, max);
|
|
}
|
|
AABB AABB::Merge(const AABB& a, const AABB& b)
|
|
{
|
|
return AABB(wi::math::Min(a.getMin(), b.getMin()), wi::math::Max(a.getMax(), b.getMax()));
|
|
}
|
|
void AABB::AddPoint(const XMFLOAT3& pos)
|
|
{
|
|
_min = wi::math::Min(_min, pos);
|
|
_max = wi::math::Max(_max, pos);
|
|
}
|
|
void AABB::AddPoint(const XMVECTOR& P)
|
|
{
|
|
XMFLOAT3 p;
|
|
XMStoreFloat3(&p, P);
|
|
AddPoint(p);
|
|
}
|
|
void AABB::Serialize(wi::Archive& archive, wi::ecs::EntitySerializer& seri)
|
|
{
|
|
if (archive.IsReadMode())
|
|
{
|
|
archive >> _min;
|
|
archive >> _max;
|
|
|
|
if (archive.GetVersion() >= 69)
|
|
{
|
|
archive >> layerMask;
|
|
archive >> userdata;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
archive << _min;
|
|
archive << _max;
|
|
|
|
if (archive.GetVersion() >= 69)
|
|
{
|
|
archive << layerMask;
|
|
archive << userdata;
|
|
}
|
|
}
|
|
}
|
|
XMFLOAT4 AABB::ProjectToScreen(const XMMATRIX& ViewProjection) const
|
|
{
|
|
XMVECTOR SCREEN_MIN = XMVectorSet(1000000, 1000000, 1000000, 1000000);
|
|
XMVECTOR SCREEN_MAX = XMVectorSet(-1000000, -1000000, -1000000, -1000000);
|
|
XMVECTOR MUL = XMVectorSet(0.5f, -0.5f, 1, 1);
|
|
XMVECTOR ADD = XMVectorSet(0.5f, 0.5f, 0, 0);
|
|
for (int i = 0; i < 8; ++i)
|
|
{
|
|
XMFLOAT3 c = corner(i);
|
|
XMVECTOR C = XMLoadFloat3(&c);
|
|
C = XMVector3TransformCoord(C, ViewProjection); // world -> clip
|
|
C = XMVectorMultiplyAdd(C, MUL, ADD); // clip -> uv
|
|
SCREEN_MIN = XMVectorMin(SCREEN_MIN, C);
|
|
SCREEN_MAX = XMVectorMax(SCREEN_MAX, C);
|
|
}
|
|
|
|
XMFLOAT4 ret;
|
|
ret.x = XMVectorGetX(SCREEN_MIN);
|
|
ret.y = XMVectorGetY(SCREEN_MIN);
|
|
ret.z = XMVectorGetX(SCREEN_MAX);
|
|
ret.w = XMVectorGetY(SCREEN_MAX);
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
bool Sphere::intersects(const XMVECTOR& P) const
|
|
{
|
|
float distsq = wi::math::DistanceSquared(XMLoadFloat3(¢er), P);
|
|
float radiussq = radius * radius;
|
|
return distsq < radiussq;
|
|
}
|
|
bool Sphere::intersects(const XMFLOAT3& P) const
|
|
{
|
|
float distsq = wi::math::DistanceSquared(center, P);
|
|
float radiussq = radius * radius;
|
|
return distsq < radiussq;
|
|
}
|
|
bool Sphere::intersects(const AABB& b) const
|
|
{
|
|
if (!b.IsValid())
|
|
return false;
|
|
XMFLOAT3 min = b.getMin();
|
|
XMFLOAT3 max = b.getMax();
|
|
XMFLOAT3 closestPointInAabb = wi::math::Min(wi::math::Max(center, min), max);
|
|
float distanceSquared = wi::math::DistanceSquared(closestPointInAabb, center);
|
|
return distanceSquared < (radius * radius);
|
|
}
|
|
bool Sphere::intersects(const Sphere& b)const
|
|
{
|
|
float dist = 0;
|
|
return intersects(b, dist);
|
|
}
|
|
bool Sphere::intersects(const Sphere& b, float& dist) const
|
|
{
|
|
dist = wi::math::Distance(center, b.center);
|
|
dist = dist - radius - b.radius;
|
|
return dist < 0;
|
|
}
|
|
bool Sphere::intersects(const Sphere& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
XMVECTOR A = XMLoadFloat3(¢er);
|
|
XMVECTOR B = XMLoadFloat3(&b.center);
|
|
XMVECTOR DIR = A - B;
|
|
XMVECTOR DIST = XMVector3Length(DIR);
|
|
DIR = DIR / DIST;
|
|
XMStoreFloat3(&direction, DIR);
|
|
dist = XMVectorGetX(DIST);
|
|
dist = dist - radius - b.radius;
|
|
return dist < 0;
|
|
}
|
|
bool Sphere::intersects(const Capsule& b) const
|
|
{
|
|
float dist = 0;
|
|
return intersects(b, dist);
|
|
}
|
|
bool Sphere::intersects(const Capsule& b, float& dist) const
|
|
{
|
|
XMVECTOR A = XMLoadFloat3(&b.base);
|
|
XMVECTOR B = XMLoadFloat3(&b.tip);
|
|
XMVECTOR N = XMVector3Normalize(A - B);
|
|
A -= N * b.radius;
|
|
B += N * b.radius;
|
|
XMVECTOR C = XMLoadFloat3(¢er);
|
|
dist = wi::math::GetPointSegmentDistance(C, A, B);
|
|
dist = dist - radius - b.radius;
|
|
return dist < 0;
|
|
}
|
|
bool Sphere::intersects(const Capsule& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
XMVECTOR A = XMLoadFloat3(&b.base);
|
|
XMVECTOR B = XMLoadFloat3(&b.tip);
|
|
XMVECTOR N = XMVector3Normalize(A - B);
|
|
A -= N * b.radius;
|
|
B += N * b.radius;
|
|
XMVECTOR C = XMLoadFloat3(¢er);
|
|
XMVECTOR D = C - wi::math::ClosestPointOnLineSegment(A, B, C);
|
|
dist = XMVectorGetX(XMVector3Length(D));
|
|
D /= dist;
|
|
XMStoreFloat3(&direction, D);
|
|
dist = dist - radius - b.radius;
|
|
return dist < 0;
|
|
}
|
|
bool Sphere::intersects(const Plane& b) const
|
|
{
|
|
return b.intersects(*this);
|
|
}
|
|
bool Sphere::intersects(const Plane& b, float& dist) const
|
|
{
|
|
return b.intersects(*this, dist);
|
|
}
|
|
bool Sphere::intersects(const Plane& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
return b.intersects(*this, dist, direction);
|
|
}
|
|
bool Sphere::intersects(const Ray& b) const
|
|
{
|
|
float dist;
|
|
XMFLOAT3 direction;
|
|
return intersects(b, dist, direction);
|
|
}
|
|
bool Sphere::intersects(const Ray& b, float& dist) const
|
|
{
|
|
XMFLOAT3 direction;
|
|
return intersects(b, dist, direction);
|
|
}
|
|
bool Sphere::intersects(const Ray& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
XMVECTOR C = XMLoadFloat3(¢er);
|
|
XMVECTOR O = XMLoadFloat3(&b.origin);
|
|
XMVECTOR D = XMLoadFloat3(&b.direction);
|
|
XMVECTOR OC = O - C;
|
|
float B = XMVectorGetX(XMVector3Dot(OC, D));
|
|
float c = XMVectorGetX(XMVector3Dot(OC, OC)) - radius * radius;
|
|
float discr = B * B - c;
|
|
if (discr > 0)
|
|
{
|
|
float discrSq = std::sqrt(discr);
|
|
|
|
float t = (-B - discrSq);
|
|
if (t<b.TMax && t>b.TMin)
|
|
{
|
|
XMVECTOR P = O + D * t;
|
|
XMVECTOR N = XMVector3Normalize(P - C);
|
|
dist = t;
|
|
XMStoreFloat3(&direction, N);
|
|
return true;
|
|
}
|
|
|
|
t = (-B + discrSq);
|
|
if (t<b.TMax && t>b.TMin)
|
|
{
|
|
XMVECTOR P = O + D * t;
|
|
XMVECTOR N = XMVector3Normalize(P - C);
|
|
dist = t;
|
|
XMStoreFloat3(&direction, N);
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
XMFLOAT4X4 Sphere::GetPlacementOrientation(const XMFLOAT3& position, const XMFLOAT3& normal) const
|
|
{
|
|
XMVECTOR N = XMVector3Normalize(XMLoadFloat3(&normal));
|
|
XMVECTOR P = XMLoadFloat3(&position);
|
|
XMVECTOR E = XMLoadFloat3(¢er) - P;
|
|
XMVECTOR T = XMVector3Normalize(XMVector3Cross(N, P - E));
|
|
XMVECTOR B = XMVector3Normalize(XMVector3Cross(T, N));
|
|
N = XMVectorSetW(N, 0);
|
|
T = XMVectorSetW(T, 0);
|
|
B = XMVectorSetW(B, 0);
|
|
P = XMVectorSetW(P, 1);
|
|
XMMATRIX M = { T, N, B, P };
|
|
XMFLOAT4X4 orientation;
|
|
XMStoreFloat4x4(&orientation, M);
|
|
return orientation;
|
|
}
|
|
|
|
|
|
|
|
bool Capsule::intersects(const Capsule& other, XMFLOAT3& position, XMFLOAT3& incident_normal, float& penetration_depth) const
|
|
{
|
|
if (getAABB().intersects(other.getAABB()) == AABB::INTERSECTION_TYPE::OUTSIDE)
|
|
return false;
|
|
|
|
XMVECTOR a_Base = XMLoadFloat3(&base);
|
|
XMVECTOR a_Tip = XMLoadFloat3(&tip);
|
|
XMVECTOR a_Radius = XMVectorReplicate(radius);
|
|
XMVECTOR a_Normal = XMVector3Normalize(a_Tip - a_Base);
|
|
XMVECTOR a_LineEndOffset = a_Normal * a_Radius;
|
|
XMVECTOR a_A = a_Base + a_LineEndOffset;
|
|
XMVECTOR a_B = a_Tip - a_LineEndOffset;
|
|
|
|
XMVECTOR b_Base = XMLoadFloat3(&other.base);
|
|
XMVECTOR b_Tip = XMLoadFloat3(&other.tip);
|
|
XMVECTOR b_Radius = XMVectorReplicate(other.radius);
|
|
XMVECTOR b_Normal = XMVector3Normalize(b_Tip - b_Base);
|
|
XMVECTOR b_LineEndOffset = b_Normal * b_Radius;
|
|
XMVECTOR b_A = b_Base + b_LineEndOffset;
|
|
XMVECTOR b_B = b_Tip - b_LineEndOffset;
|
|
|
|
// Vectors between line endpoints:
|
|
XMVECTOR v0 = b_A - a_A;
|
|
XMVECTOR v1 = b_B - a_A;
|
|
XMVECTOR v2 = b_A - a_B;
|
|
XMVECTOR v3 = b_B - a_B;
|
|
|
|
// Distances (squared) between line endpoints:
|
|
float d0 = XMVectorGetX(XMVector3LengthSq(v0));
|
|
float d1 = XMVectorGetX(XMVector3LengthSq(v1));
|
|
float d2 = XMVectorGetX(XMVector3LengthSq(v2));
|
|
float d3 = XMVectorGetX(XMVector3LengthSq(v3));
|
|
|
|
// Select best potential endpoint on capsule A:
|
|
XMVECTOR bestA;
|
|
if (d2 < d0 || d2 < d1 || d3 < d0 || d3 < d1)
|
|
{
|
|
bestA = a_B;
|
|
}
|
|
else
|
|
{
|
|
bestA = a_A;
|
|
}
|
|
|
|
// Select point on capsule B line segment nearest to best potential endpoint on A capsule:
|
|
XMVECTOR bestB = wi::math::ClosestPointOnLineSegment(b_A, b_B, bestA);
|
|
|
|
// Now do the same for capsule A segment:
|
|
bestA = wi::math::ClosestPointOnLineSegment(a_A, a_B, bestB);
|
|
|
|
// Finally, sphere collision:
|
|
XMVECTOR N = bestA - bestB;
|
|
XMVECTOR len = XMVector3Length(N);
|
|
N /= len;
|
|
float dist = XMVectorGetX(len);
|
|
|
|
XMStoreFloat3(&position, bestA - N * radius);
|
|
XMStoreFloat3(&incident_normal, N);
|
|
penetration_depth = radius + other.radius - dist;
|
|
|
|
return penetration_depth > 0;
|
|
}
|
|
bool Capsule::intersects(const Sphere& b) const
|
|
{
|
|
return b.intersects(*this);
|
|
}
|
|
bool Capsule::intersects(const Sphere& b, float& dist) const
|
|
{
|
|
bool intersects = b.intersects(*this, dist);
|
|
dist = -dist;
|
|
return intersects;
|
|
}
|
|
bool Capsule::intersects(const Sphere& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
bool intersects = b.intersects(*this, dist, direction);
|
|
dist = -dist;
|
|
direction.x *= -1;
|
|
direction.y *= -1;
|
|
direction.z *= -1;
|
|
return intersects;
|
|
}
|
|
bool Capsule::intersects(const Plane& b) const
|
|
{
|
|
return b.intersects(*this);
|
|
}
|
|
bool Capsule::intersects(const Plane& b, float& dist) const
|
|
{
|
|
bool intersects = b.intersects(*this, dist);
|
|
dist = -dist;
|
|
return intersects;
|
|
}
|
|
bool Capsule::intersects(const Plane& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
bool intersects = b.intersects(*this, dist, direction);
|
|
dist = -dist;
|
|
return intersects;
|
|
}
|
|
bool Capsule::intersects(const Ray& ray) const
|
|
{
|
|
float dist;
|
|
XMFLOAT3 direction;
|
|
return intersects(ray, dist, direction);
|
|
}
|
|
bool Capsule::intersects(const Ray& ray, float& dist) const
|
|
{
|
|
XMFLOAT3 direction;
|
|
return intersects(ray, dist, direction);
|
|
}
|
|
bool Capsule::intersects(const Ray& ray, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
XMVECTOR A = XMLoadFloat3(&base);
|
|
XMVECTOR B = XMLoadFloat3(&tip);
|
|
XMVECTOR L = XMVector3Normalize(A - B);
|
|
A -= L * radius;
|
|
B += L * radius;
|
|
XMVECTOR O = XMLoadFloat3(&ray.origin);
|
|
XMVECTOR D = XMLoadFloat3(&ray.direction);
|
|
XMVECTOR C = XMVector3Normalize(XMVector3Cross(L, A - O));
|
|
XMVECTOR N = XMVector3Cross(L, C);
|
|
XMVECTOR Plane = XMPlaneFromPointNormal(A, N);
|
|
XMVECTOR I = XMPlaneIntersectLine(Plane, O, O + D * ray.TMax);
|
|
XMVECTOR P = wi::math::ClosestPointOnLineSegment(A, B, I);
|
|
|
|
Sphere sphere;
|
|
XMStoreFloat3(&sphere.center, P);
|
|
sphere.radius = radius;
|
|
return sphere.intersects(ray, dist, direction);
|
|
}
|
|
bool Capsule::intersects(const XMFLOAT3& point) const
|
|
{
|
|
XMVECTOR Base = XMLoadFloat3(&base);
|
|
XMVECTOR Tip = XMLoadFloat3(&tip);
|
|
XMVECTOR Radius = XMVectorReplicate(radius);
|
|
XMVECTOR Normal = XMVector3Normalize(Tip - Base);
|
|
XMVECTOR LineEndOffset = Normal * Radius;
|
|
XMVECTOR A = Base + LineEndOffset;
|
|
XMVECTOR B = Tip - LineEndOffset;
|
|
|
|
XMVECTOR P = XMLoadFloat3(&point);
|
|
|
|
XMVECTOR C = wi::math::ClosestPointOnLineSegment(A, B, P);
|
|
|
|
return XMVectorGetX(XMVector3Length(P - C)) <= radius;
|
|
}
|
|
XMFLOAT4X4 Capsule::GetPlacementOrientation(const XMFLOAT3& position, const XMFLOAT3& normal) const
|
|
{
|
|
const XMVECTOR Base = XMLoadFloat3(&base);
|
|
const XMVECTOR Tip = XMLoadFloat3(&tip);
|
|
const XMVECTOR Axis = XMVector3Normalize(Tip - Base);
|
|
XMVECTOR N = XMVector3Normalize(XMLoadFloat3(&normal));
|
|
XMVECTOR P = XMLoadFloat3(&position);
|
|
XMVECTOR E = Axis;
|
|
XMVECTOR T = XMVector3Normalize(XMVector3Cross(N, P - E));
|
|
XMVECTOR B = XMVector3Normalize(XMVector3Cross(T, N));
|
|
N = XMVectorSetW(N, 0);
|
|
T = XMVectorSetW(T, 0);
|
|
B = XMVectorSetW(B, 0);
|
|
P = XMVectorSetW(P, 1);
|
|
XMMATRIX M = { T, N, B, P };
|
|
XMFLOAT4X4 orientation;
|
|
XMStoreFloat4x4(&orientation, M);
|
|
return orientation;
|
|
}
|
|
|
|
|
|
|
|
bool Plane::intersects(const Sphere& b) const
|
|
{
|
|
float dist;
|
|
XMFLOAT3 direction;
|
|
return intersects(b, dist, direction);
|
|
}
|
|
bool Plane::intersects(const Sphere& b, float& dist) const
|
|
{
|
|
XMFLOAT3 direction;
|
|
return intersects(b, dist, direction);
|
|
}
|
|
bool Plane::intersects(const Sphere& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
XMVECTOR C = XMLoadFloat3(&b.center);
|
|
dist = wi::math::GetPlanePointDistance(XMLoadFloat3(&origin), XMLoadFloat3(&normal), C);
|
|
direction = normal;
|
|
if (dist < 0)
|
|
{
|
|
direction.x *= -1;
|
|
direction.y *= -1;
|
|
direction.z *= -1;
|
|
dist = std::abs(dist);
|
|
}
|
|
dist = dist - b.radius;
|
|
if (dist < 0)
|
|
{
|
|
XMMATRIX planeProjection = XMLoadFloat4x4(&projection);
|
|
XMVECTOR clipSpacePos = XMVector3Transform(C, planeProjection);
|
|
XMVECTOR uvw = clipSpacePos * XMVectorSet(0.5f, -0.5f, 0.5f, 1) + XMVectorSet(0.5f, 0.5f, 0.5f, 0);
|
|
XMVECTOR uvw_sat = XMVectorSaturate(uvw);
|
|
XMVECTOR uvw_diff = XMVectorAbs(uvw - uvw_sat);
|
|
if (XMVectorGetX(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
dist = 0; // force no collision
|
|
else if (XMVectorGetY(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
dist = 0; // force no collision
|
|
else if (XMVectorGetZ(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
dist = 0; // force no collision
|
|
}
|
|
return dist < 0;
|
|
}
|
|
bool Plane::intersects(const Capsule& b) const
|
|
{
|
|
float dist;
|
|
XMFLOAT3 direction;
|
|
return intersects(b, dist, direction);
|
|
}
|
|
bool Plane::intersects(const Capsule& b, float& dist) const
|
|
{
|
|
XMFLOAT3 direction;
|
|
return intersects(b, dist, direction);
|
|
}
|
|
bool Plane::intersects(const Capsule& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
direction = normal;
|
|
dist = 0;
|
|
|
|
XMVECTOR N = XMLoadFloat3(&normal);
|
|
XMVECTOR O = XMLoadFloat3(&origin);
|
|
|
|
XMVECTOR A = XMLoadFloat3(&b.base);
|
|
XMVECTOR B = XMLoadFloat3(&b.tip);
|
|
XMVECTOR D = XMVector3Normalize(A - B);
|
|
A -= D * b.radius;
|
|
B += D * b.radius;
|
|
|
|
XMVECTOR C;
|
|
if (std::abs(XMVectorGetX(XMVector3Dot(N, D))) < std::numeric_limits<float>::epsilon())
|
|
{
|
|
// parallel line-plane, take any point on capsule segment
|
|
C = A;
|
|
}
|
|
else
|
|
{
|
|
// trace point on plane by capsule line and compute closest point on capsule to intersection point
|
|
XMVECTOR t = XMVector3Dot(N, (A - O) / XMVectorAbs(XMVector3Dot(N, D)));
|
|
XMVECTOR LinePlaneIntersection = A + D * t;
|
|
C = wi::math::ClosestPointOnLineSegment(A, B, LinePlaneIntersection);
|
|
}
|
|
|
|
dist = wi::math::GetPlanePointDistance(O, N, C);
|
|
|
|
if (dist < 0)
|
|
{
|
|
direction.x *= -1;
|
|
direction.y *= -1;
|
|
direction.z *= -1;
|
|
dist = std::abs(dist);
|
|
}
|
|
|
|
dist = dist - b.radius;
|
|
|
|
if (dist < 0)
|
|
{
|
|
XMMATRIX planeProjection = XMLoadFloat4x4(&projection);
|
|
XMVECTOR clipSpacePos = XMVector3Transform(C, planeProjection);
|
|
XMVECTOR uvw = clipSpacePos * XMVectorSet(0.5f, -0.5f, 0.5f, 1) + XMVectorSet(0.5f, 0.5f, 0.5f, 0);
|
|
XMVECTOR uvw_sat = XMVectorSaturate(uvw);
|
|
XMVECTOR uvw_diff = XMVectorAbs(uvw - uvw_sat);
|
|
if (XMVectorGetX(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
dist = 0; // force no collision
|
|
else if (XMVectorGetY(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
dist = 0; // force no collision
|
|
else if (XMVectorGetZ(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
dist = 0; // force no collision
|
|
}
|
|
return dist < 0;
|
|
}
|
|
bool Plane::intersects(const Ray& b) const
|
|
{
|
|
float dist;
|
|
XMFLOAT3 direction;
|
|
return intersects(b, dist, direction);
|
|
}
|
|
bool Plane::intersects(const Ray& b, float& dist) const
|
|
{
|
|
XMFLOAT3 direction;
|
|
return intersects(b, dist, direction);
|
|
}
|
|
bool Plane::intersects(const Ray& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
dist = 0;
|
|
direction = normal;
|
|
|
|
XMVECTOR N = XMLoadFloat3(&normal);
|
|
XMVECTOR D = XMLoadFloat3(&b.direction);
|
|
if (std::abs(XMVectorGetX(XMVector3Dot(N, D))) < std::numeric_limits<float>::epsilon())
|
|
return false; // parallel line-plane
|
|
|
|
XMVECTOR O = XMLoadFloat3(&b.origin);
|
|
XMVECTOR A = O + D * b.TMin;
|
|
XMVECTOR B = O + D * b.TMax;
|
|
|
|
dist = XMVectorGetX(XMVector3Dot(N, (XMLoadFloat3(&origin) - O) / XMVector3Dot(N, D))); // plane intersection
|
|
if (dist <= 0)
|
|
return false;
|
|
|
|
XMVECTOR C = O + D * dist;
|
|
|
|
XMMATRIX planeProjection = XMLoadFloat4x4(&projection);
|
|
XMVECTOR clipSpacePos = XMVector3Transform(C, planeProjection);
|
|
XMVECTOR uvw = clipSpacePos * XMVectorSet(0.5f, -0.5f, 0.5f, 1) + XMVectorSet(0.5f, 0.5f, 0.5f, 0);
|
|
XMVECTOR uvw_sat = XMVectorSaturate(uvw);
|
|
XMVECTOR uvw_diff = XMVectorAbs(uvw - uvw_sat);
|
|
if (XMVectorGetX(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
return false; // force no collision
|
|
else if (XMVectorGetY(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
return false; // force no collision
|
|
else if (XMVectorGetZ(uvw_diff) > std::numeric_limits<float>::epsilon())
|
|
return false; // force no collision
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool Ray::intersects(const AABB& b) const
|
|
{
|
|
return b.intersects(*this);
|
|
}
|
|
bool Ray::intersects(const Sphere& b) const
|
|
{
|
|
return b.intersects(*this);
|
|
}
|
|
bool Ray::intersects(const Sphere& b, float& dist) const
|
|
{
|
|
bool intersects = b.intersects(*this, dist);
|
|
return intersects;
|
|
}
|
|
bool Ray::intersects(const Sphere& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
bool intersects = b.intersects(*this, dist, direction);
|
|
return intersects;
|
|
}
|
|
bool Ray::intersects(const Capsule& b) const
|
|
{
|
|
return b.intersects(*this);
|
|
}
|
|
bool Ray::intersects(const Capsule& b, float& dist) const
|
|
{
|
|
bool intersects = b.intersects(*this, dist);
|
|
return intersects;
|
|
}
|
|
bool Ray::intersects(const Capsule& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
bool intersects = b.intersects(*this, dist, direction);
|
|
return intersects;
|
|
}
|
|
bool Ray::intersects(const Plane& b) const
|
|
{
|
|
return b.intersects(*this);
|
|
}
|
|
bool Ray::intersects(const Plane& b, float& dist) const
|
|
{
|
|
return b.intersects(*this, dist);
|
|
}
|
|
bool Ray::intersects(const Plane& b, float& dist, XMFLOAT3& direction) const
|
|
{
|
|
return b.intersects(*this, dist, direction);
|
|
}
|
|
void Ray::CreateFromPoints(const XMFLOAT3& a, const XMFLOAT3& b)
|
|
{
|
|
XMVECTOR A = XMLoadFloat3(&a);
|
|
XMVECTOR B = XMLoadFloat3(&b);
|
|
XMVECTOR D = B - A;
|
|
XMVECTOR L = XMVector3Length(D);
|
|
D /= L;
|
|
XMStoreFloat3(&origin, A);
|
|
XMStoreFloat3(&direction, D);
|
|
XMStoreFloat3(&direction_inverse, XMVectorReciprocal(D));
|
|
TMin = 0;
|
|
TMax = XMVectorGetX(L);
|
|
}
|
|
XMFLOAT4X4 Ray::GetPlacementOrientation(const XMFLOAT3& position, const XMFLOAT3& normal) const
|
|
{
|
|
XMVECTOR N = XMVector3Normalize(XMLoadFloat3(&normal));
|
|
XMVECTOR P = XMLoadFloat3(&position);
|
|
XMVECTOR E = XMLoadFloat3(&origin);
|
|
XMVECTOR T = XMVector3Normalize(XMVector3Cross(N, P - E));
|
|
XMVECTOR B = XMVector3Normalize(XMVector3Cross(T, N));
|
|
N = XMVectorSetW(N, 0);
|
|
T = XMVectorSetW(T, 0);
|
|
B = XMVectorSetW(B, 0);
|
|
P = XMVectorSetW(P, 1);
|
|
XMMATRIX M = { T, N, B, P };
|
|
XMFLOAT4X4 orientation;
|
|
XMStoreFloat4x4(&orientation, M);
|
|
return orientation;
|
|
}
|
|
|
|
|
|
|
|
|
|
void Frustum::Create(const XMMATRIX& viewProjection)
|
|
{
|
|
// We are interested in columns of the matrix, so transpose because we can access only rows:
|
|
const XMMATRIX mat = XMMatrixTranspose(viewProjection);
|
|
|
|
// Near plane:
|
|
XMStoreFloat4(&planes[0], XMPlaneNormalize(mat.r[2]));
|
|
|
|
// Far plane:
|
|
XMStoreFloat4(&planes[1], XMPlaneNormalize(mat.r[3] - mat.r[2]));
|
|
|
|
// Left plane:
|
|
XMStoreFloat4(&planes[2], XMPlaneNormalize(mat.r[3] + mat.r[0]));
|
|
|
|
// Right plane:
|
|
XMStoreFloat4(&planes[3], XMPlaneNormalize(mat.r[3] - mat.r[0]));
|
|
|
|
// Top plane:
|
|
XMStoreFloat4(&planes[4], XMPlaneNormalize(mat.r[3] - mat.r[1]));
|
|
|
|
// Bottom plane:
|
|
XMStoreFloat4(&planes[5], XMPlaneNormalize(mat.r[3] + mat.r[1]));
|
|
}
|
|
|
|
bool Frustum::CheckPoint(const XMFLOAT3& point) const
|
|
{
|
|
XMVECTOR p = XMLoadFloat3(&point);
|
|
for (short i = 0; i < 6; i++)
|
|
{
|
|
if (XMVectorGetX(XMPlaneDotCoord(XMLoadFloat4(&planes[i]), p)) < 0.0f)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
bool Frustum::CheckSphere(const XMFLOAT3& center, float radius) const
|
|
{
|
|
int i;
|
|
XMVECTOR c = XMLoadFloat3(¢er);
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
if (XMVectorGetX(XMPlaneDotCoord(XMLoadFloat4(&planes[i]), c)) < -radius)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
Frustum::BoxFrustumIntersect Frustum::CheckBox(const AABB& box) const
|
|
{
|
|
if (!box.IsValid())
|
|
return BOX_FRUSTUM_OUTSIDE;
|
|
int iTotalIn = 0;
|
|
for (int p = 0; p < 6; ++p)
|
|
{
|
|
|
|
int iInCount = 8;
|
|
int iPtIn = 1;
|
|
|
|
for (int i = 0; i < 8; ++i)
|
|
{
|
|
XMFLOAT3 C = box.corner(i);
|
|
if (XMVectorGetX(XMPlaneDotCoord(XMLoadFloat4(&planes[p]), XMLoadFloat3(&C))) < 0.0f)
|
|
{
|
|
iPtIn = 0;
|
|
--iInCount;
|
|
}
|
|
}
|
|
if (iInCount == 0)
|
|
return BOX_FRUSTUM_OUTSIDE;
|
|
iTotalIn += iPtIn;
|
|
}
|
|
if (iTotalIn == 6)
|
|
return(BOX_FRUSTUM_INSIDE);
|
|
return(BOX_FRUSTUM_INTERSECTS);
|
|
}
|
|
bool Frustum::CheckBoxFast(const AABB& box) const
|
|
{
|
|
if (!box.IsValid())
|
|
return false;
|
|
XMVECTOR max = XMLoadFloat3(&box._max);
|
|
XMVECTOR min = XMLoadFloat3(&box._min);
|
|
XMVECTOR zero = XMVectorZero();
|
|
for (size_t p = 0; p < 6; ++p)
|
|
{
|
|
XMVECTOR plane = XMLoadFloat4(&planes[p]);
|
|
XMVECTOR lt = XMVectorLess(plane, zero);
|
|
XMVECTOR furthestFromPlane = XMVectorSelect(max, min, lt);
|
|
if (XMVectorGetX(XMPlaneDotCoord(plane, furthestFromPlane)) < 0.0f)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
const XMFLOAT4& Frustum::getNearPlane() const { return planes[0]; }
|
|
const XMFLOAT4& Frustum::getFarPlane() const { return planes[1]; }
|
|
const XMFLOAT4& Frustum::getLeftPlane() const { return planes[2]; }
|
|
const XMFLOAT4& Frustum::getRightPlane() const { return planes[3]; }
|
|
const XMFLOAT4& Frustum::getTopPlane() const { return planes[4]; }
|
|
const XMFLOAT4& Frustum::getBottomPlane() const { return planes[5]; }
|
|
|
|
|
|
|
|
bool Hitbox2D::intersects(const XMFLOAT2& b) const
|
|
{
|
|
if (pos.x + siz.x < b.x)
|
|
return false;
|
|
else if (pos.x > b.x)
|
|
return false;
|
|
else if (pos.y + siz.y < b.y)
|
|
return false;
|
|
else if (pos.y > b.y)
|
|
return false;
|
|
return true;
|
|
}
|
|
bool Hitbox2D::intersects(const Hitbox2D& b) const
|
|
{
|
|
return wi::math::Collision2D(pos, siz, b.pos, b.siz);
|
|
}
|
|
|
|
}
|