330 lines
8.8 KiB
C++
330 lines
8.8 KiB
C++
#include "Math.h"
|
|
|
|
namespace Math
|
|
{
|
|
|
|
float Abs(float v)
|
|
{
|
|
return fabs(v);
|
|
}
|
|
|
|
float Pow(float Base, float Exponent)
|
|
{
|
|
return pow(Base, Exponent);
|
|
}
|
|
|
|
float Distance(const CVector3f& A, const CVector3f& B)
|
|
{
|
|
return sqrtf( Pow(B.x - A.x, 2.f) +
|
|
Pow(B.y - A.y, 2.f) +
|
|
Pow(B.z - A.z, 2.f) );
|
|
}
|
|
|
|
std::pair<bool,float> RayPlaneIntersecton(const CRay& ray, const CPlane& plane)
|
|
{
|
|
// Code based on ray/plane intersect code from Ogre
|
|
// https://bitbucket.org/sinbad/ogre/src/197116fd2ac62c57cdeed1666f9866c3dddd4289/OgreMain/src/OgreMath.cpp?at=default#OgreMath.cpp-350
|
|
|
|
// Are ray and plane parallel?
|
|
float denom = plane.Normal().Dot(ray.Direction());
|
|
|
|
if (Abs(denom) < FLT_EPSILON)
|
|
return std::pair<bool,float>(false, 0.f);
|
|
|
|
// Not parallel
|
|
float nom = plane.Normal().Dot(ray.Origin()) + plane.Dist();
|
|
float t = -(nom / denom);
|
|
return std::pair<bool,float>(t >= 0.f, t);
|
|
}
|
|
|
|
std::pair<bool,float> RayBoxIntersection(const CRay& Ray, const CAABox& Box)
|
|
{
|
|
// Code slightly modified from Ogre
|
|
// https://github.com/ehsan/ogre/blob/master/OgreMain/src/OgreMath.cpp
|
|
if (Box.IsNull()) return std::pair<bool,float>(false, 0);
|
|
if (Box.IsInfinite()) return std::pair<bool,float>(true, 0);
|
|
|
|
float lowt = 0.0f;
|
|
float t;
|
|
bool Hit = false;
|
|
CVector3f HitPoint;
|
|
const CVector3f& RayOrig = Ray.Origin();
|
|
const CVector3f& RayDir = Ray.Direction();
|
|
const CVector3f Min = Box.Min();
|
|
const CVector3f Max = Box.Max();
|
|
|
|
// Check origin inside first
|
|
if ( RayOrig > Min && RayOrig < Max )
|
|
{
|
|
return std::pair<bool, float>(true, 0);
|
|
}
|
|
|
|
// Check each face in turn, only check closest 3
|
|
// Min x
|
|
if (RayOrig.x <= Min.x && RayDir.x > 0)
|
|
{
|
|
t = (Min.x - RayOrig.x) / RayDir.x;
|
|
if (t >= 0)
|
|
{
|
|
// Substitute t back into ray and check bounds and dist
|
|
HitPoint = RayOrig + RayDir * t;
|
|
if (HitPoint.y >= Min.y && HitPoint.y <= Max.y &&
|
|
HitPoint.z >= Min.z && HitPoint.z <= Max.z &&
|
|
(!Hit || t < lowt))
|
|
{
|
|
Hit = true;
|
|
lowt = t;
|
|
}
|
|
}
|
|
}
|
|
// Max x
|
|
if (RayOrig.x >= Max.x && RayDir.x < 0)
|
|
{
|
|
t = (Max.x - RayOrig.x) / RayDir.x;
|
|
if (t >= 0)
|
|
{
|
|
// Substitute t back into ray and check bounds and dist
|
|
HitPoint = RayOrig + RayDir * t;
|
|
if (HitPoint.y >= Min.y && HitPoint.y <= Max.y &&
|
|
HitPoint.z >= Min.z && HitPoint.z <= Max.z &&
|
|
(!Hit || t < lowt))
|
|
{
|
|
Hit = true;
|
|
lowt = t;
|
|
}
|
|
}
|
|
}
|
|
// Min y
|
|
if (RayOrig.y <= Min.y && RayDir.y > 0)
|
|
{
|
|
t = (Min.y - RayOrig.y) / RayDir.y;
|
|
if (t >= 0)
|
|
{
|
|
// Substitute t back into ray and check bounds and dist
|
|
HitPoint = RayOrig + RayDir * t;
|
|
if (HitPoint.x >= Min.x && HitPoint.x <= Max.x &&
|
|
HitPoint.z >= Min.z && HitPoint.z <= Max.z &&
|
|
(!Hit || t < lowt))
|
|
{
|
|
Hit = true;
|
|
lowt = t;
|
|
}
|
|
}
|
|
}
|
|
// Max y
|
|
if (RayOrig.y >= Max.y && RayDir.y < 0)
|
|
{
|
|
t = (Max.y - RayOrig.y) / RayDir.y;
|
|
if (t >= 0)
|
|
{
|
|
// Substitute t back into ray and check bounds and dist
|
|
HitPoint = RayOrig + RayDir * t;
|
|
if (HitPoint.x >= Min.x && HitPoint.x <= Max.x &&
|
|
HitPoint.z >= Min.z && HitPoint.z <= Max.z &&
|
|
(!Hit || t < lowt))
|
|
{
|
|
Hit = true;
|
|
lowt = t;
|
|
}
|
|
}
|
|
}
|
|
// Min z
|
|
if (RayOrig.z <= Min.z && RayDir.z > 0)
|
|
{
|
|
t = (Min.z - RayOrig.z) / RayDir.z;
|
|
if (t >= 0)
|
|
{
|
|
// Substitute t back into ray and check bounds and dist
|
|
HitPoint = RayOrig + RayDir * t;
|
|
if (HitPoint.x >= Min.x && HitPoint.x <= Max.x &&
|
|
HitPoint.y >= Min.y && HitPoint.y <= Max.y &&
|
|
(!Hit || t < lowt))
|
|
{
|
|
Hit = true;
|
|
lowt = t;
|
|
}
|
|
}
|
|
}
|
|
// Max z
|
|
if (RayOrig.z >= Max.z && RayDir.z < 0)
|
|
{
|
|
t = (Max.z - RayOrig.z) / RayDir.z;
|
|
if (t >= 0)
|
|
{
|
|
// Substitute t back into ray and check bounds and dist
|
|
HitPoint = RayOrig + RayDir * t;
|
|
if (HitPoint.x >= Min.x && HitPoint.x <= Max.x &&
|
|
HitPoint.y >= Min.y && HitPoint.y <= Max.y &&
|
|
(!Hit || t < lowt))
|
|
{
|
|
Hit = true;
|
|
lowt = t;
|
|
}
|
|
}
|
|
}
|
|
|
|
return std::pair<bool,float>(Hit, lowt);
|
|
}
|
|
|
|
std::pair<bool,float> RayLineIntersection(const CRay& ray, const CVector3f& pointA,
|
|
const CVector3f& pointB, float threshold)
|
|
{
|
|
// http://geomalgorithms.com/a07-_distance.html
|
|
// http://www.gamedev.net/topic/589705-rayline-intersection-in-3d/
|
|
CVector3f u = ray.Direction();
|
|
CVector3f v = pointB - pointA;
|
|
CVector3f w = ray.Origin() - pointA;
|
|
float a = u.Dot(u);
|
|
float b = u.Dot(v);
|
|
float c = v.Dot(v);
|
|
float d = u.Dot(w);
|
|
float e = v.Dot(w);
|
|
float D = a * c - b * b;
|
|
float sc, sN, sD = D;
|
|
float tc, tN, tD = D;
|
|
|
|
if (D < FLT_EPSILON) {
|
|
sN = 0.f;
|
|
sD = 1.f;
|
|
tN = e;
|
|
tD = c;
|
|
}
|
|
else {
|
|
sN = b * e - c * d;
|
|
tN = a * e - b * d;
|
|
|
|
if (sN < 0.f) {
|
|
sN = 0.f;
|
|
tN = e;
|
|
tD = c;
|
|
}
|
|
}
|
|
|
|
if (tN < 0.f) {
|
|
tN = 0.f;
|
|
|
|
if (-d < 0.f)
|
|
sN = 0.f;
|
|
else {
|
|
sN = -d;
|
|
sD = a;
|
|
}
|
|
}
|
|
else if (tN > tD) {
|
|
tN = tD;
|
|
|
|
if (-d + b < 0.f)
|
|
sN = 0.f;
|
|
else {
|
|
sN = -d + b;
|
|
sD = a;
|
|
}
|
|
}
|
|
|
|
sc = (fabs(sN) < FLT_EPSILON ? 0.f : sN / sD);
|
|
tc = (fabs(tN) < FLT_EPSILON ? 0.f : tN / tD);
|
|
|
|
CVector3f dP = w + (u * sc) - (v * tc);
|
|
bool hit = (dP.Magnitude() <= threshold);
|
|
return std::pair<bool,float>(hit, sc);
|
|
}
|
|
|
|
std::pair<bool,float> RayTriangleIntersection(const CRay& Ray,
|
|
const CVector3f& vtxA, const CVector3f& vtxB,
|
|
const CVector3f& vtxC, bool AllowBackfaces)
|
|
{
|
|
// Ogre code cuz I'm lazy and bad at math
|
|
// https://github.com/ehsan/ogre/blob/master/OgreMain/src/OgreMath.cpp#L709
|
|
CVector3f FaceNormal = (vtxB - vtxA).Cross(vtxC - vtxA);
|
|
|
|
//
|
|
// Calculate intersection with plane.
|
|
//
|
|
float t;
|
|
{
|
|
float denom = FaceNormal.Dot(Ray.Direction());
|
|
|
|
// Check intersect side
|
|
if (denom > + std::numeric_limits<float>::epsilon())
|
|
{
|
|
if (!AllowBackfaces)
|
|
return std::pair<bool,float>(false, 0);
|
|
}
|
|
else if (denom < - std::numeric_limits<float>::epsilon())
|
|
{
|
|
if (false)
|
|
return std::pair<bool,float>(false, 0);
|
|
}
|
|
else
|
|
{
|
|
// Parallel or triangle area is close to zero when
|
|
// the plane normal not normalised.
|
|
return std::pair<bool,float>(false, 0);
|
|
}
|
|
|
|
t = FaceNormal.Dot(vtxA - Ray.Origin()) / denom;
|
|
|
|
if (t < 0)
|
|
{
|
|
// Intersection is behind origin
|
|
return std::pair<bool,float>(false, 0);
|
|
}
|
|
}
|
|
|
|
//
|
|
// Calculate the largest area projection plane in X, Y or Z.
|
|
//
|
|
size_t i0, i1;
|
|
{
|
|
float n0 = fabs(FaceNormal[0]);
|
|
float n1 = fabs(FaceNormal[1]);
|
|
float n2 = fabs(FaceNormal[2]);
|
|
|
|
i0 = 1; i1 = 2;
|
|
if (n1 > n2)
|
|
{
|
|
if (n1 > n0) i0 = 0;
|
|
}
|
|
else
|
|
{
|
|
if (n2 > n0) i1 = 0;
|
|
}
|
|
}
|
|
|
|
//
|
|
// Check the intersection point is inside the triangle.
|
|
//
|
|
{
|
|
float u1 = vtxB[i0] - vtxA[i0];
|
|
float v1 = vtxB[i1] - vtxA[i1];
|
|
float u2 = vtxC[i0] - vtxA[i0];
|
|
float v2 = vtxC[i1] - vtxA[i1];
|
|
float u0 = t * Ray.Direction()[i0] + Ray.Origin()[i0] - vtxA[i0];
|
|
float v0 = t * Ray.Direction()[i1] + Ray.Origin()[i1] - vtxA[i1];
|
|
|
|
float alpha = u0 * v2 - u2 * v0;
|
|
float beta = u1 * v0 - u0 * v1;
|
|
float area = u1 * v2 - u2 * v1;
|
|
|
|
// epsilon to avoid float precision error
|
|
const float EPSILON = 1e-6f;
|
|
|
|
float tolerance = - EPSILON * area;
|
|
|
|
if (area > 0)
|
|
{
|
|
if (alpha < tolerance || beta < tolerance || alpha+beta > area-tolerance)
|
|
return std::pair<bool,float>(false, 0);
|
|
}
|
|
else
|
|
{
|
|
if (alpha > tolerance || beta > tolerance || alpha+beta < area-tolerance)
|
|
return std::pair<bool,float>(false, 0);
|
|
}
|
|
}
|
|
|
|
return std::pair<bool,float>(true, t);
|
|
}
|
|
|
|
} // End namespace
|