diff --git a/include/Nazara/Math/Ray.hpp b/include/Nazara/Math/Ray.hpp index d342dcc26..d47663e88 100644 --- a/include/Nazara/Math/Ray.hpp +++ b/include/Nazara/Math/Ray.hpp @@ -1,4 +1,4 @@ -// Copyright (C) 2014 Rémi Bèges - Jérôme Leclercq +// Copyright (C) 2014 Gawaboumga (https://github.com/Gawaboumga) - 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 @@ -10,51 +10,57 @@ #include #include #include +#include #include #include #include #include -template class NzRay +template +class NzRay { public: NzRay() = default; NzRay(T X, T Y, T Z, T directionX, T directionY, T directionZ); NzRay(const T origin[3], const T direction[3]); - NzRay(const NzVector3& origin, const NzVector3& direction); NzRay(const NzPlane& planeOne, const NzPlane& planeTwo); - template explicit NzRay(const NzVector3& origin, const NzVector3& direction); + NzRay(const NzVector3& origin, const NzVector3& direction); template explicit NzRay(const NzRay& ray); + template explicit NzRay(const NzVector3& origin, const NzVector3& direction); NzRay(const NzRay& ray) = default; ~NzRay() = default; - NzVector3 GetClosestPoint(const NzVector3& point) const; + T ClosestPoint(const NzVector3& point) const; + NzVector3 GetPoint(T lambda) const; - bool Intersect(const NzBox& box, NzVector3 * hitPoint = nullptr, NzVector3 * hitSecondPoint = nullptr) const; - bool Intersect(const NzOrientedBox& orientedBox, NzVector3 * hitPoint = nullptr, NzVector3 * hitSecondPoint = nullptr) const; - bool Intersect(const NzPlane& plane, NzVector3 * hitPoint = nullptr) const; - bool Intersect(const NzSphere& sphere, NzVector3 * hitPoint = nullptr, NzVector3 * hitSecondPoint = nullptr) const; + //bool Intersect(const NzBoundingVolume& volume, T* closestHit = nullptr, T* farthestHit = nullptr) const; + bool Intersect(const NzBox& box, T* closestHit = nullptr, T* farthestHit = nullptr) const; + bool Intersect(const NzBox& box, const NzMatrix4& transform, T* closestHit = nullptr, T* farthestHit = nullptr) const; + //bool Intersect(const NzOrientedBox& orientedBox, T* closestHit = nullptr, T* farthestHit = nullptr) const; + bool Intersect(const NzPlane& plane, T* hit = nullptr) const; + bool Intersect(const NzSphere& sphere, T* closestHit = nullptr, T* farthestHit = nullptr) const; - NzVector3 operator*(T lambda) const; + NzRay& MakeAxisX(); + NzRay& MakeAxisY(); + NzRay& MakeAxisZ(); NzRay& Set(T X, T Y, T Z, T directionX, T directionY, T directionZ); NzRay& Set(const T origin[3], const T direction[3]); - NzRay& Set(const NzVector3& origin, const NzVector3& direction); NzRay& Set(const NzPlane& planeOne, const NzPlane& planeTwo); - template NzRay& Set(const NzVector3& origin, const NzVector3& direction); + NzRay& Set(const NzRay& ray); + NzRay& Set(const NzVector3& origin, const NzVector3& direction); template NzRay& Set(const NzRay& ray); - NzRay& Set(const NzRay& ray); - - NzRay& SetDirection(const NzVector3& direction); - NzRay& SetOrigin(const NzVector3& origin); + template NzRay& Set(const NzVector3& origin, const NzVector3& direction); NzString ToString() const; + NzVector3 operator*(T lambda) const; + + static NzRay AxisX(); + static NzRay AxisY(); + static NzRay AxisZ(); static NzRay Lerp(const NzRay& from, const NzRay& to, T interpolation); - static NzRay UnitX(); - static NzRay UnitY(); - static NzRay UnitZ(); NzVector3 direction, origin; }; diff --git a/include/Nazara/Math/Ray.inl b/include/Nazara/Math/Ray.inl index 1b423f900..d0c74092e 100644 --- a/include/Nazara/Math/Ray.inl +++ b/include/Nazara/Math/Ray.inl @@ -1,8 +1,9 @@ -// Copyright (C) 2014 Jérôme Leclercq +// Copyright (C) 2014 Gawaboumga (https://github.com/Gawaboumga) - 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 +#include #include #define F(a) static_cast(a) @@ -10,30 +11,23 @@ template NzRay::NzRay(T X, T Y, T Z, T DirectionX, T DirectionY, T DirectionZ) { - Set(X, Y, Z, DirectionX, DirectionY, DirectionZ); + Set(X, Y, Z, DirectionX, DirectionY, DirectionZ); } template NzRay::NzRay(const T Origin[3], const T Direction[3]) { - Set(Origin, Direction); -} - -template -NzRay::NzRay(const NzVector3& Origin, const NzVector3& Direction) -{ - Set(Origin, Direction); + Set(Origin, Direction); } template NzRay::NzRay(const NzPlane& planeOne, const NzPlane& planeTwo) { - Set(planeOne, planeTwo); + Set(planeOne, planeTwo); } template -template -NzRay::NzRay(const NzVector3& Origin, const NzVector3& Direction) +NzRay::NzRay(const NzVector3& Origin, const NzVector3& Direction) { Set(Origin, Direction); } @@ -46,219 +40,294 @@ NzRay::NzRay(const NzRay& ray) } template -NzVector3 NzRay::GetClosestPoint(const NzVector3& point) const +template +NzRay::NzRay(const NzVector3& Origin, const NzVector3& Direction) { - NzVector3 delta = point - origin; - T vsq = direction.GetSquaredLength(); - T proj = delta.DotProduct(direction); + Set(Origin, Direction); +} - return GetPoint(proj/vsq); +template +T NzRay::ClosestPoint(const NzVector3& point) const +{ + NzVector3 delta = point - origin; + T vsq = direction.GetSquaredLength(); + T proj = delta.DotProduct(direction); + + return proj/vsq; } template NzVector3 NzRay::GetPoint(T lambda) const { - return NzVector3(origin + direction * lambda); + return origin + lambda*direction; } - +/* template -bool NzRay::Intersect(const NzBox& box, NzVector3 * hitPoint, NzVector3 * hitSecondPoint) const +bool NzRay::Intersect(const NzBoundingVolume& volume, T* closestHit, T* farthestHit) 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))) + switch (volume.extend) { - NazaraWarning("Division by zero !"); // The algorithm is still correct. + case nzExtend_Finite: + { + if (Intersect(volume.aabb)) + return Intersect(volume.obb, closestHit, farthestHit); + + return false; + } + + case nzExtend_Infinite: + { + if (closestHit) + *closestHit = F(0.0); + + if (farthestHit) + *farthestHit = std::numeric_limits::infinity(); + + return true; + } + + case nzExtend_Null: + return false; } - #endif - T tx1 = (box.x - origin.x) / direction.x; - T tx2 = (box.x + box.width - origin.x) / direction.x; + NazaraError("Invalid extend type (0x" + NzString::Number(volume.extend, 16) + ')'); + return false; +} +*/ +template +bool NzRay::Intersect(const NzBox& box, T* closestHit, T* farthestHit) const +{ + // http://www.gamedev.net/topic/429443-obb-ray-and-obb-plane-intersection/ + T tfirst = F(0.0); + T tlast = std::numeric_limits::infinity(); - T tmin = std::min(tx1, tx2); - T tmax = std::max(tx1, tx2); + NzVector3 boxMin = box.GetMinimum(); + NzVector3 boxMax = box.GetMaximum(); - T ty1 = (box.y - origin.y) / direction.y; - T ty2 = (box.y + box.height - origin.y) / direction.y; + for (unsigned int i = 0; i < 3; ++i) + { + T dir = direction[i]; + T ori = origin[i]; + T max = boxMax[i]; + T min = boxMin[i]; - tmin = std::max(tmin, std::min(ty1, ty2)); - tmax = std::min(tmax, std::max(ty1, ty2)); + if (NzNumberEquals(dir, F(0.0))) + { + if (ori < max && ori > min) + continue; - T tz1 = (box.z - origin.z) / direction.z; - T tz2 = (box.z + box.depth - origin.z) / direction.z; + return false; + } - tmin = std::max(tmin, std::min(tz1, tz2)); - tmax = std::min(tmax, std::max(tz1, tz2)); + T tmin = (min - ori) / dir; + T tmax = (max - ori) / dir; + if (tmin > tmax) + std::swap(tmin, tmax); - if (hitPoint) - hitPoint->Set(GetPoint(tmin)); - if (hitSecondPoint) - hitSecondPoint->Set(GetPoint(tmax)); + if (tmax < tfirst || tmin > tlast) + return false; - return tmax >= std::max(F(0.0), tmin) && tmin < INFINITY; + tfirst = std::max(tfirst, tmin); + tlast = std::min(tlast, tmax); + } + + if (closestHit) + *closestHit = tfirst; + + if (farthestHit) + *farthestHit = tlast; + + return true; } template -bool NzRay::Intersect(const NzOrientedBox& orientedBox, NzVector3 * hitPoint, NzVector3 * hitSecondPoint) const +bool NzRay::Intersect(const NzBox& box, const NzMatrix4& transform, T* closestHit, T* farthestHit) const +{ + // http://www.opengl-tutorial.org/miscellaneous/clicking-on-objects/picking-with-custom-ray-obb-function/ + // Intersection method from Real-Time Rendering and Essential Mathematics for Games + T tMin = F(0.0); + T tMax = std::numeric_limits::infinity(); + + NzVector3 boxMin = box.GetMinimum(); + NzVector3 boxMax = box.GetMaximum(); + NzVector3 delta = transform.GetTranslation() - origin; + + // Test intersection with the 2 planes perpendicular to the OBB's X axis + for (unsigned int i = 0; i < 3; ++i) + { + NzVector3 axis(transform(0, i), transform(1, i), transform(2, i)); + T e = axis.DotProduct(delta); + T f = direction.DotProduct(axis); + + if (!NzNumberEquals(f, F(0.0))) + { + T t1 = (e + boxMin[i]) / f; // Intersection with the "left" plane + T t2 = (e + boxMax[i]) / 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) + std::swap(t1, 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. + 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 + boxMin[i] > F(0.0) || -e + boxMax[i] < F(0.0)) + return false; + } + + if (closestHit) + *closestHit = tMin; + + if (farthestHit) + *farthestHit = tMax; + + return true; +} + +///FIXME: Le test ci-dessous est beaucoup trop approximatif pour être vraiment utile +/// Mais le vrai problème vient certainement des OrientedBox en elles-mêmes, peut-être faut-il envisager de les refaire ? +/* +template +bool NzRay::Intersect(const NzOrientedBox& orientedBox, T* closestHit, T* farthestHit) const { NzVector3 width = (orientedBox.GetCorner(nzCorner_NearLeftBottom) - orientedBox.GetCorner(nzCorner_FarLeftBottom)).Normalize(); NzVector3 height = (orientedBox.GetCorner(nzCorner_FarLeftTop) - orientedBox.GetCorner(nzCorner_FarLeftBottom)).Normalize(); NzVector3 depth = (orientedBox.GetCorner(nzCorner_FarRightBottom) - orientedBox.GetCorner(nzCorner_FarLeftBottom)).Normalize(); - // Construction of the inverse of the matrix who did the rotation -> orthogonal matrix. - NzMatrix4 transformation(width.x, height.x, depth.x, F(0.0), - width.y, height.y, depth.y, F(0.0), - width.z, height.z, depth.z, F(0.0), - F(0.0), F(0.0), F(0.0), F(1.0)); + // Construction de la matrice de transformation de l'OBB + NzMatrix4 matrix(width.x, height.x, depth.x, F(0.0), + width.y, height.y, depth.y, F(0.0), + width.z, height.z, depth.z, F(0.0), + F(0.0), F(0.0), F(0.0), F(1.0)); + matrix.Transpose(); - // Reduction to aabb problem - NzVector3 newOrigin = transformation.Transform(origin); - NzVector3 newDirection = transformation.Transform(direction); + // Test en tant qu'AABB avec une matrice de rotation + return Intersect(orientedBox.localBox, matrix, closestHit, farthestHit); +} +*/ +template +bool NzRay::Intersect(const NzPlane& plane, T* hit) const +{ + T divisor = plane.normal.DotProduct(direction); + if (NzNumberEquals(divisor, F(0.0))) + return false; // perpendicular - NzVector3 tmp, tmp2; - if (NzRay(newOrigin, newDirection).Intersect(NzBox(orientedBox.GetCorner(nzCorner_NearRightTop), orientedBox.GetCorner(nzCorner_FarLeftBottom)), &tmp, &tmp2)) - { - if (hitPoint) - { - transformation.Transpose(); - hitPoint->Set(transformation.Transform(tmp)); - if (hitSecondPoint) - hitSecondPoint->Set(transformation.Transform(tmp2)); - } + T lambda = -(plane.normal.DotProduct(origin) - plane.distance) / divisor; // The plane is ax+by+cz=d + if (lambda < F(0.0)) + return false; // Le plan est derrière le rayon - return true; - } + if (hit) + *hit = lambda; - return false; + return true; } template -bool NzRay::Intersect(const NzPlane& plane, NzVector3 * hitPoint) const +bool NzRay::Intersect(const NzSphere& sphere, T* closestHit, T* farthestHit) const { - T divisor = plane.normal.DotProduct(direction); + NzVector3 sphereRay = sphere.GetPosition() - origin; + T length = sphereRay.DotProduct(direction); - if (NzNumberEquals(divisor, F(0.0))) - return false; // perpendicular + if (length < F(0.0)) + return false; // ray is perpendicular to the vector origin - center - if (!hitPoint) - return true; + T squaredDistance = sphereRay.GetSquaredLength() - length*length; + T squaredRadius = sphere.radius*sphere.radius; - T lambda = - (plane.normal.DotProduct(origin) - plane.distance) / divisor; // The plane is ax+by+cz=d - hitPoint->Set(GetPoint(lambda)); + if (squaredDistance > squaredRadius) + return false; // if the ray is further than the radius - return true; + // Calcul des points d'intersection si besoin + if (closestHit || farthestHit) + { + T deltaLambda = std::sqrt(squaredRadius - squaredDistance); + + if (closestHit) + *closestHit = length - deltaLambda; + + if (farthestHit) + *farthestHit = length + deltaLambda; + } + + return true; } template -bool NzRay::Intersect(const NzSphere& sphere, NzVector3 * hitPoint, NzVector3 * hitSecondPoint) const +NzRay& NzRay::MakeAxisX() { - NzVector3 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; + return Set(NzVector3::Zero(), NzVector3::UnitX()); } template -NzVector3 NzRay::operator*(T lambda) const +NzRay& NzRay::MakeAxisY() { - return GetPoint(lambda); + return Set(NzVector3::Zero(), NzVector3::UnitY()); +} + +template +NzRay& NzRay::MakeAxisZ() +{ + return Set(NzVector3::Zero(), NzVector3::UnitZ()); } template NzRay& NzRay::Set(T X, T Y, T Z, T directionX, T directionY, T directionZ) { - direction = NzVector3(directionX, directionY, directionZ); - origin = NzVector3(X, Y, Z); + direction.Set(directionX, directionY, directionZ); + origin.Set(X, Y, Z); - return *this; + return *this; } template NzRay& NzRay::Set(const T Origin[3], const T Direction[3]) { - direction = NzVector3(Direction); - origin = NzVector3(Origin); + direction.Set(Direction); + origin.Set(Origin); - return *this; -} - -template -NzRay& NzRay::Set(const NzVector3& Origin, const NzVector3& Direction) -{ - direction = Direction; - origin = Origin; - - return *this; + return *this; } template NzRay& NzRay::Set(const NzPlane& planeOne, const NzPlane& planeTwo) { - T termOne = planeOne.normal.GetLength(); - T termTwo = planeOne.normal.DotProduct(planeTwo.normal); - T termFour = planeTwo.normal.GetLength(); - T det = termOne * termFour - termTwo * termTwo; + 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))) - { + #if NAZARA_MATH_SAFE + if (NzNumberEquals(det, F(0.0))) + { + NzString error("Planes are parallel."); - NzString error("Planes are parallel."); + NazaraError(error); + throw std::domain_error(error); + } + #endif - 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; - 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; - direction = planeOne.normal.CrossProduct(planeTwo.normal); - origin = planeOne.normal * fc0 + planeTwo.normal * fc1; - - return *this; -} - -template -template -NzRay& NzRay::Set(const NzVector3& Origin, const NzVector3& Direction) -{ - direction = NzVector3(Direction); - origin = NzVector3(Origin); - - return *this; -} - -template -template -NzRay& NzRay::Set(const NzRay& ray) -{ - direction = NzVector3(ray.direction); - origin = NzVector3(ray.origin); - - return *this; + return *this; } template @@ -270,19 +339,32 @@ NzRay& NzRay::Set(const NzRay& ray) } template -NzRay& NzRay::SetDirection(const NzVector3& Direction) +NzRay& NzRay::Set(const NzVector3& Origin, const NzVector3& Direction) { - direction = Direction; + direction = Direction; + origin = Origin; - return *this; + return *this; } template -NzRay& NzRay::SetOrigin(const NzVector3& Origin) +template +NzRay& NzRay::Set(const NzRay& ray) { - origin = Origin; + direction.Set(ray.direction); + origin.Set(ray.origin); - return *this; + return *this; +} + +template +template +NzRay& NzRay::Set(const NzVector3& Origin, const NzVector3& Direction) +{ + direction.Set(Direction); + origin.Set(Origin); + + return *this; } template @@ -293,30 +375,45 @@ NzString NzRay::ToString() const return ss << "Ray(origin: " << origin.ToString() << ", direction: " << direction.ToString() << ")"; } +template +NzVector3 NzRay::operator*(T lambda) const +{ + return GetPoint(lambda); +} + +template +NzRay NzRay::AxisX() +{ + NzRay axis; + axis.MakeAxisX(); + + return axis; +} + +template +NzRay NzRay::AxisY() +{ + NzRay axis; + axis.MakeAxisY(); + + return axis; +} + +template +NzRay NzRay::AxisZ() +{ + NzRay axis; + axis.MakeAxisZ(); + + return axis; +} + template NzRay NzRay::Lerp(const NzRay& from, const NzRay& to, T interpolation) { return NzRay(from.origin.Lerp(to.origin, interpolation), from.direction.Lerp(to.direction, interpolation)); } -template -NzRay NzRay::UnitX() -{ - return NzRay(NzVector3::Zero(), NzVector3::UnitX()); -} - -template -NzRay NzRay::UnitY() -{ - return NzRay(NzVector3::Zero(), NzVector3::UnitY()); -} - -template -NzRay NzRay::UnitZ() -{ - return NzRay(NzVector3::Zero(), NzVector3::UnitZ()); -} - template std::ostream& operator<<(std::ostream& out, const NzRay& ray) {