416 lines
11 KiB
C++
416 lines
11 KiB
C++
// Copyright (C) 2014 Jérôme Leclercq
|
|
// This file is part of the "Nazara Engine - Mathematics module"
|
|
// For conditions of distribution and use, see copyright notice in Config.hpp
|
|
|
|
#include <Nazara/Core/StringStream.hpp>
|
|
#include <Nazara/Core/Debug.hpp>
|
|
|
|
#define F(a) static_cast<T>(a)
|
|
|
|
template<typename T>
|
|
NzRay<T>::NzRay(T X, T Y, T Z, T DirectionX, T DirectionY, T DirectionZ)
|
|
{
|
|
Set(X, Y, Z, DirectionX, DirectionY, DirectionZ);
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>::NzRay(const T Origin[3], const T Direction[3])
|
|
{
|
|
Set(Origin, Direction);
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>::NzRay(const NzVector3<T>& Origin, const NzVector3<T>& Direction)
|
|
{
|
|
Set(Origin, Direction);
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>::NzRay(const NzPlane<T>& planeOne, const NzPlane<T>& planeTwo)
|
|
{
|
|
Set(planeOne, planeTwo);
|
|
}
|
|
|
|
template<typename T>
|
|
template<typename U>
|
|
NzRay<T>::NzRay(const NzVector3<U>& Origin, const NzVector3<U>& Direction)
|
|
{
|
|
Set(Origin, Direction);
|
|
}
|
|
|
|
template<typename T>
|
|
template<typename U>
|
|
NzRay<T>::NzRay(const NzRay<U>& ray)
|
|
{
|
|
Set(ray);
|
|
}
|
|
|
|
template<typename T>
|
|
NzVector3<T> NzRay<T>::GetClosestPoint(const NzVector3<T>& point) const
|
|
{
|
|
NzVector3<T> delta = point - origin;
|
|
T vsq = direction.GetSquaredLength();
|
|
T proj = delta.DotProduct(direction);
|
|
|
|
return GetPoint(proj/vsq);
|
|
}
|
|
|
|
template<typename T>
|
|
NzVector3<T> NzRay<T>::GetDirection() const
|
|
{
|
|
return direction;
|
|
}
|
|
|
|
template<typename T>
|
|
NzVector3<T> NzRay<T>::GetOrigin() const
|
|
{
|
|
return origin;
|
|
}
|
|
|
|
template<typename T>
|
|
NzVector3<T> NzRay<T>::GetPoint(T lambda) const
|
|
{
|
|
return NzVector3<T>(origin + direction * lambda);
|
|
}
|
|
|
|
template<typename T>
|
|
bool NzRay<T>::Intersect(const NzBox<T>& box, NzVector3<T> * hitPoint, NzVector3<T> * hitSecondPoint) const
|
|
{
|
|
// Slab method
|
|
|
|
#if NAZARA_MATH_SAFE
|
|
if (NzNumberEquals(direction.x, F(0.0)) || NzNumberEquals(direction.y, F(0.0)) || NzNumberEquals(direction.z, F(0.0)))
|
|
{
|
|
NazaraWarning("Division by zero !"); // The algorithm is still correct.
|
|
}
|
|
#endif
|
|
|
|
T tx1 = (box.x - origin.x) / direction.x;
|
|
T tx2 = (box.x + box.width - origin.x) / direction.x;
|
|
|
|
T tmin = std::min(tx1, tx2);
|
|
T tmax = std::max(tx1, tx2);
|
|
|
|
T ty1 = (box.y - origin.y) / direction.y;
|
|
T ty2 = (box.y + box.height - origin.y) / direction.y;
|
|
|
|
tmin = std::max(tmin, std::min(ty1, ty2));
|
|
tmax = std::min(tmax, std::max(ty1, ty2));
|
|
|
|
T tz1 = (box.z - origin.z) / direction.z;
|
|
T tz2 = (box.z + box.depth - origin.z) / direction.z;
|
|
|
|
tmin = std::max(tmin, std::min(tz1, tz2));
|
|
tmax = std::min(tmax, std::max(tz1, tz2));
|
|
|
|
if (hitPoint)
|
|
hitPoint->Set(GetPoint(tmin));
|
|
if (hitSecondPoint)
|
|
hitSecondPoint->Set(GetPoint(tmax));
|
|
|
|
return tmax >= std::max(F(0.0), tmin) && tmin < INFINITY;
|
|
}
|
|
|
|
template<typename T>
|
|
bool NzRay<T>::Intersect(const NzOrientedBox<T>& orientedBox, const NzMatrix4<T>& matrix, NzVector3<T> * hitPoint, NzVector3<T> * hitSecondPoint) const
|
|
{
|
|
// Intersection method from Real-Time Rendering and Essential Mathematics for Games
|
|
|
|
T tMin = F(0.0);
|
|
T tMax = INFINITY;
|
|
|
|
NzVector3<T> OBBposition_worldspace(matrix[3].x, matrix[3].y, matrix[3].z);
|
|
|
|
NzVector3<T> delta = OBBposition_worldspace - origin;
|
|
|
|
// Test intersection with the 2 planes perpendicular to the OBB's X axis
|
|
NzVector3<T> xaxis(matrix[0].x, matrix[0].y, matrix[0].z);
|
|
T e = xaxis.DotProduct(delta);
|
|
T f = direction.DotProduct(xaxis);
|
|
|
|
if (std::abs(f) > F(0.0))
|
|
{ // Standard case
|
|
|
|
T t1 = (e + orientedBox.localBox.x) / f; // Intersection with the "left" plane
|
|
T t2 = (e + (orientedBox.localBox.x + orientedBox.localBox.width)) / f; // Intersection with the "right" plane
|
|
// t1 and t2 now contain distances betwen ray origin and ray-plane intersections
|
|
|
|
// We want t1 to represent the nearest intersection,
|
|
// so if it's not the case, invert t1 and t2
|
|
if (t1 > t2)
|
|
{ T w = t1; t1 = t2; t2 = w; } // swap t1 and t2
|
|
|
|
// tMax is the nearest "far" intersection (amongst the X,Y and Z planes pairs)
|
|
if (t2 < tMax)
|
|
tMax = t2;
|
|
// tMin is the farthest "near" intersection (amongst the X,Y and Z planes pairs)
|
|
if (t1 > tMin)
|
|
tMin = t1;
|
|
|
|
// And here's the trick :
|
|
// If "far" is closer than "near", then there is NO intersection.
|
|
// See the images in the tutorials for the visual explanation.
|
|
if (tMax < tMin)
|
|
return false;
|
|
}
|
|
else
|
|
// Rare case : the ray is almost parallel to the planes, so they don't have any "intersection"
|
|
if (-e + orientedBox.localBox.x > F(0.0) || -e + (orientedBox.localBox.x + orientedBox.localBox.width) < F(0.0))
|
|
return false;
|
|
|
|
// Test intersection with the 2 planes perpendicular to the OBB's Y axis
|
|
// Exactly the same thing than above.
|
|
NzVector3<T> yaxis(matrix[1].x, matrix[1].y, matrix[1].z);
|
|
e = yaxis.DotProduct(delta);
|
|
f = direction.DotProduct(yaxis);
|
|
|
|
if (std::abs(f) > F(0.0))
|
|
{
|
|
|
|
T t1 = (e + orientedBox.localBox.y) / f;
|
|
T t2 = (e + (orientedBox.localBox.y + orientedBox.localBox.height)) / f;
|
|
|
|
if (t1 > t2)
|
|
{ T w = t1; t1 = t2; t2 = w; } // swap t1 and t2
|
|
|
|
if (t2 < tMax)
|
|
tMax = t2;
|
|
if (t1 > tMin)
|
|
tMin = t1;
|
|
if (tMin > tMax)
|
|
return false;
|
|
|
|
}
|
|
else
|
|
if (-e + orientedBox.localBox.y > F(0.0) || -e + (orientedBox.localBox.y + orientedBox.localBox.height) < F(0.0))
|
|
return false;
|
|
|
|
|
|
// Test intersection with the 2 planes perpendicular to the OBB's Z axis
|
|
// Exactly the same thing than above.
|
|
NzVector3<T> zaxis(matrix[2].x, matrix[2].y, matrix[2].z);
|
|
e = zaxis.DotProduct(delta);
|
|
f = direction.DotProduct(zaxis);
|
|
|
|
if (std::abs(f) > F(0.0))
|
|
{
|
|
T t1 = (e + orientedBox.localBox.z) / f;
|
|
T t2 = (e + (orientedBox.localBox.z + orientedBox.localBox.depth)) / f;
|
|
|
|
if (t1 > t2)
|
|
{ T w = t1; t1 = t2; t2 = w; } // swap t1 and t2
|
|
|
|
if (t2 < tMax)
|
|
tMax = t2;
|
|
if (t1 > tMin)
|
|
tMin = t1;
|
|
if (tMin > tMax)
|
|
return false;
|
|
|
|
}
|
|
else
|
|
if (-e + orientedBox.localBox.z > F(0.0) || -e + (orientedBox.localBox.z + orientedBox.localBox.depth) < F(0.0))
|
|
return false;
|
|
|
|
if (hitPoint)
|
|
hitPoint->Set(GetPoint(tMin));
|
|
if (hitSecondPoint)
|
|
hitSecondPoint->Set(GetPoint(tMax));
|
|
|
|
return true;
|
|
}
|
|
|
|
template<typename T>
|
|
bool NzRay<T>::Intersect(const NzPlane<T>& plane, NzVector3<T> * hitPoint) const
|
|
{
|
|
T divisor = plane.normal.DotProduct(direction);
|
|
|
|
if (NzNumberEquals(divisor, F(0.0)))
|
|
return false; // perpendicular
|
|
|
|
if (!hitPoint)
|
|
return true;
|
|
|
|
T lambda = - (plane.normal.DotProduct(origin) - plane.distance) / divisor; // The plane is ax+by+cz=d
|
|
hitPoint->Set(GetPoint(lambda));
|
|
|
|
return true;
|
|
}
|
|
|
|
template<typename T>
|
|
bool NzRay<T>::Intersect(const NzSphere<T>& sphere, NzVector3<T> * hitPoint, NzVector3<T> * hitSecondPoint) const
|
|
{
|
|
NzVector3<T> distanceCenterOrigin = sphere.GetPosition() - origin;
|
|
T length = distanceCenterOrigin.DotProduct(direction);
|
|
|
|
if (length < F(0.0))
|
|
return false; // ray is perpendicular to the vector origin - center
|
|
|
|
T squaredDistance = distanceCenterOrigin.GetSquaredLength() - length * length;
|
|
|
|
T squaredRadius = sphere.GetRadius() * sphere.GetRadius();
|
|
|
|
if (squaredDistance > squaredRadius)
|
|
return false; // if the ray is further than the radius
|
|
|
|
if (!hitPoint)
|
|
return true;
|
|
|
|
T deltaLambda = std::sqrt(squaredRadius - squaredDistance);
|
|
|
|
if (hitPoint)
|
|
hitPoint->Set(GetPoint(length - deltaLambda));
|
|
if (hitSecondPoint)
|
|
hitSecondPoint->Set(GetPoint(length + deltaLambda));
|
|
|
|
return true;
|
|
}
|
|
|
|
template<typename T>
|
|
NzVector3<T> NzRay<T>::operator*(T lambda) const
|
|
{
|
|
return GetPoint(lambda);
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>& NzRay<T>::Set(T X, T Y, T Z, T directionX, T directionY, T directionZ)
|
|
{
|
|
direction = NzVector3<T>(directionX, directionY, directionZ);
|
|
origin = NzVector3<T>(X, Y, Z);
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>& NzRay<T>::Set(const T Origin[3], const T Direction[3])
|
|
{
|
|
direction = NzVector3<T>(Direction);
|
|
origin = NzVector3<T>(Origin);
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>& NzRay<T>::Set(const NzVector3<T>& Origin, const NzVector3<T>& Direction)
|
|
{
|
|
direction = Direction;
|
|
origin = Origin;
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>& NzRay<T>::Set(const NzPlane<T>& planeOne, const NzPlane<T>& planeTwo)
|
|
{
|
|
T termOne = planeOne.normal.GetLength();
|
|
T termTwo = planeOne.normal.DotProduct(planeTwo.normal);
|
|
T termFour = planeTwo.normal.GetLength();
|
|
T det = termOne * termFour - termTwo * termTwo;
|
|
|
|
#if NAZARA_MATH_SAFE
|
|
if (NzNumberEquals(det, F(0.0)))
|
|
{
|
|
|
|
NzString error("Planes are parallel.");
|
|
|
|
NazaraError(error);
|
|
throw std::domain_error(error);
|
|
}
|
|
#endif
|
|
|
|
T invdet = F(1.0) / det;
|
|
T fc0 = (termFour * -planeOne.distance + termTwo * planeTwo.distance) * invdet;
|
|
T fc1 = (termOne * -planeTwo.distance + termTwo * planeOne.distance) * invdet;
|
|
|
|
direction = planeOne.normal.CrossProduct(planeTwo.normal);
|
|
origin = planeOne.normal * fc0 + planeTwo.normal * fc1;
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
template<typename U>
|
|
NzRay<T>& NzRay<T>::Set(const NzVector3<U>& Origin, const NzVector3<U>& Direction)
|
|
{
|
|
direction = NzVector3<T>(Direction);
|
|
origin = NzVector3<T>(Origin);
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
template<typename U>
|
|
NzRay<T>& NzRay<T>::Set(const NzRay<U>& ray)
|
|
{
|
|
direction = NzVector3<T>(ray.direction);
|
|
origin = NzVector3<T>(ray.origin);
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>& NzRay<T>::Set(const NzRay& ray)
|
|
{
|
|
std::memcpy(this, &ray, sizeof(NzRay));
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>& NzRay<T>::SetDirection(const NzVector3<T>& Direction)
|
|
{
|
|
direction = Direction;
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T>& NzRay<T>::SetOrigin(const NzVector3<T>& Origin)
|
|
{
|
|
origin = Origin;
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<typename T>
|
|
NzString NzRay<T>::ToString() const
|
|
{
|
|
NzStringStream ss;
|
|
|
|
return ss << "Ray(" << origin.x << ", " << origin.y << ", " << origin.z << " | direction: " << direction.x << ", " << direction.y << ", " << direction.z << ')';
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T> NzRay<T>::Lerp(const NzRay& from, const NzRay& to, T interpolation)
|
|
{
|
|
return NzRay<T>(from.origin.Lerp(to.origin, interpolation), from.direction.Lerp(to.direction, interpolation));
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T> NzRay<T>::UnitX()
|
|
{
|
|
return NzRay(NzVector3<T>::Zero(), NzVector3<T>::UnitX());
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T> NzRay<T>::UnitY()
|
|
{
|
|
return NzRay(NzVector3<T>::Zero(), NzVector3<T>::UnitY());
|
|
}
|
|
|
|
template<typename T>
|
|
NzRay<T> NzRay<T>::UnitZ()
|
|
{
|
|
return NzRay(NzVector3<T>::Zero(), NzVector3<T>::UnitZ());
|
|
}
|
|
|
|
template<typename T>
|
|
std::ostream& operator<<(std::ostream& out, const NzRay<T>& ray)
|
|
{
|
|
return out << ray.ToString();
|
|
}
|
|
|
|
#undef F
|
|
|
|
#include <Nazara/Core/DebugOff.hpp>
|