From f3ccd60b5fccb3151fd2b36712ce2898ca9be109 Mon Sep 17 00:00:00 2001 From: Gawaboumga Date: Fri, 27 Jun 2014 19:39:51 +0200 Subject: [PATCH] New class ray Class ray who represents a ray in space and who can be used as a line. Support of classical intersections. Former-commit-id: 2ea5af0cf749dbefdd841f9b02bfab2af5058cdb --- include/Nazara/Math.hpp | 1 + include/Nazara/Math/Ray.hpp | 71 ++++++ include/Nazara/Math/Ray.inl | 415 ++++++++++++++++++++++++++++++++++++ 3 files changed, 487 insertions(+) create mode 100644 include/Nazara/Math/Ray.hpp create mode 100644 include/Nazara/Math/Ray.inl diff --git a/include/Nazara/Math.hpp b/include/Nazara/Math.hpp index e69eab49c..e70158c4a 100644 --- a/include/Nazara/Math.hpp +++ b/include/Nazara/Math.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include diff --git a/include/Nazara/Math/Ray.hpp b/include/Nazara/Math/Ray.hpp new file mode 100644 index 000000000..8b7276e6c --- /dev/null +++ b/include/Nazara/Math/Ray.hpp @@ -0,0 +1,71 @@ +// Copyright (C) 2014 Rémi Bèges - 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 + +#pragma once + +#ifndef NAZARA_RAY_HPP +#define NAZARA_RAY_HPP + +#include +#include +#include +#include +#include +#include +#include + +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); + template explicit NzRay(const NzRay& ray); + NzRay(const NzRay& ray) = default; + ~NzRay() = default; + + NzVector3 GetClosestPoint(const NzVector3& point) const; + NzVector3 GetDirection() const; + NzVector3 GetOrigin() const; + NzVector3 GetPoint(T lambda) const; + + bool Intersect(const NzBox& box, NzVector3 * hitPoint = nullptr, NzVector3 * hitSecondPoint = nullptr) const; + bool Intersect(const NzOrientedBox& orientedBox, const NzMatrix4& matrix, 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; + + NzVector3 operator*(T lambda) const; + + 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); + template NzRay& Set(const NzRay& ray); + NzRay& Set(const NzRay& ray); + + NzRay& SetDirection(const NzVector3& direction); + NzRay& SetOrigin(const NzVector3& origin); + + NzString ToString() const; + + static NzRay Lerp(const NzRay& from, const NzRay& to, T interpolation); + static NzRay UnitX(); + static NzRay UnitY(); + static NzRay UnitZ(); + + NzVector3 direction, origin; +}; + +template std::ostream& operator<<(std::ostream& out, const NzRay& vec); + +typedef NzRay NzRayd; +typedef NzRay NzRayf; + +#include + +#endif // NAZARA_RAY_HPP diff --git a/include/Nazara/Math/Ray.inl b/include/Nazara/Math/Ray.inl new file mode 100644 index 000000000..2bd72da58 --- /dev/null +++ b/include/Nazara/Math/Ray.inl @@ -0,0 +1,415 @@ +// 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 +#include + +#define F(a) static_cast(a) + +template +NzRay::NzRay(T X, T Y, T Z, T DirectionX, T DirectionY, T 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); +} + +template +NzRay::NzRay(const NzPlane& planeOne, const NzPlane& planeTwo) +{ + Set(planeOne, planeTwo); +} + +template +template +NzRay::NzRay(const NzVector3& Origin, const NzVector3& Direction) +{ + Set(Origin, Direction); +} + +template +template +NzRay::NzRay(const NzRay& ray) +{ + Set(ray); +} + +template +NzVector3 NzRay::GetClosestPoint(const NzVector3& point) const +{ + NzVector3 delta = point - origin; + T vsq = direction.GetSquaredLength(); + T proj = delta.DotProduct(direction); + + return GetPoint(proj/vsq); +} + +template +NzVector3 NzRay::GetDirection() const +{ + return direction; +} + +template +NzVector3 NzRay::GetOrigin() const +{ + return origin; +} + +template +NzVector3 NzRay::GetPoint(T lambda) const +{ + return NzVector3(origin + direction * lambda); +} + +template +bool NzRay::Intersect(const NzBox& box, NzVector3 * hitPoint, NzVector3 * 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 +bool NzRay::Intersect(const NzOrientedBox& orientedBox, const NzMatrix4& matrix, NzVector3 * hitPoint, NzVector3 * hitSecondPoint) const +{ + // Intersection method from Real-Time Rendering and Essential Mathematics for Games + + T tMin = F(0.0); + T tMax = INFINITY; + + NzVector3 OBBposition_worldspace(matrix[3].x, matrix[3].y, matrix[3].z); + + NzVector3 delta = OBBposition_worldspace - origin; + + // Test intersection with the 2 planes perpendicular to the OBB's X axis + NzVector3 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 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 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 +bool NzRay::Intersect(const NzPlane& plane, NzVector3 * 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 +bool NzRay::Intersect(const NzSphere& sphere, NzVector3 * hitPoint, NzVector3 * hitSecondPoint) const +{ + 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; +} + +template +NzVector3 NzRay::operator*(T lambda) const +{ + return GetPoint(lambda); +} + +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); + + return *this; +} + +template +NzRay& NzRay::Set(const T Origin[3], const T Direction[3]) +{ + direction = NzVector3(Direction); + origin = NzVector3(Origin); + + return *this; +} + +template +NzRay& NzRay::Set(const NzVector3& Origin, const NzVector3& Direction) +{ + direction = Direction; + origin = Origin; + + 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; + + #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 +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; +} + +template +NzRay& NzRay::Set(const NzRay& ray) +{ + std::memcpy(this, &ray, sizeof(NzRay)); + + return *this; +} + +template +NzRay& NzRay::SetDirection(const NzVector3& Direction) +{ + direction = Direction; + + return *this; +} + +template +NzRay& NzRay::SetOrigin(const NzVector3& Origin) +{ + origin = Origin; + + return *this; +} + +template +NzString NzRay::ToString() const +{ + NzStringStream ss; + + return ss << "Ray(" << origin.x << ", " << origin.y << ", " << origin.z << " | direction: " << direction.x << ", " << direction.y << ", " << direction.z << ')'; +} + +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) +{ + return out << ray.ToString(); +} + +#undef F + +#include