diff --git a/include/reactphysics3d/collision/shapes/AABB.h b/include/reactphysics3d/collision/shapes/AABB.h index 11dac1950..a4d9f9218 100644 --- a/include/reactphysics3d/collision/shapes/AABB.h +++ b/include/reactphysics3d/collision/shapes/AABB.h @@ -112,6 +112,9 @@ class AABB { /// Return true if the ray intersects the AABB bool testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInv, decimal rayMaxFraction) const; + /// Compute the intersection of a ray and the AABB + bool raycast(const Ray& ray, Vector3& hitPoint) const; + /// Apply a scale factor to the AABB void applyScale(const Vector3& scale); @@ -311,6 +314,53 @@ RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Vector3& rayOrigin, const Ve return tMax >= std::max(tMin, decimal(0.0)); } +// Compute the intersection of a ray and the AABB +RP3D_FORCE_INLINE bool AABB::raycast(const Ray& ray, Vector3& hitPoint) const { + + decimal tMin = decimal(0.0); + decimal tMax = DECIMAL_LARGEST; + + const decimal epsilon = 0.00001; + + const Vector3 rayDirection = ray.point2 - ray.point1; + + // For all three slabs + for (int i=0; i < 3; i++) { + + // If the ray is parallel to the slab + if (std::abs(rayDirection[i]) < epsilon) { + + // If origin of the ray is not inside the slab, no hit + if (ray.point1[i] < mMinCoordinates[i] || ray.point1[i] > mMaxCoordinates[i]) return false; + } + else { + + decimal rayDirectionInverse = decimal(1.0) / rayDirection[i]; + decimal t1 = (mMinCoordinates[i] - ray.point1[i]) * rayDirectionInverse; + decimal t2 = (mMaxCoordinates[i] - ray.point1[i]) * rayDirectionInverse; + + if (t1 > t2) { + + // Swap t1 and t2 + decimal tTemp = t2; + t2 = t1; + t1 = tTemp; + } + + tMin = std::max(tMin, t1); + tMax = std::min(tMax, t2); + + // Exit with no collision + if (tMin > tMax) return false; + } + } + + // Compute the hit point + hitPoint = ray.point1 + tMin * rayDirection; + + return true; +} + } #endif diff --git a/include/reactphysics3d/collision/shapes/HeightFieldShape.h b/include/reactphysics3d/collision/shapes/HeightFieldShape.h index efa2625ee..f706fc8da 100644 --- a/include/reactphysics3d/collision/shapes/HeightFieldShape.h +++ b/include/reactphysics3d/collision/shapes/HeightFieldShape.h @@ -102,6 +102,10 @@ class HeightFieldShape : public ConcaveShape { HalfEdgeStructure& triangleHalfEdgeStructure, int upAxis = 1, decimal integerHeightScale = 1.0f, const Vector3& scaling = Vector3(1,1,1)); + /// Raycast a single triangle of the height-field + bool raycastTriangle(const Ray& ray, const Vector3& p1, const Vector3& p2, const Vector3& p3, uint shapeId, + Collider *collider, RaycastInfo& raycastInfo, decimal &smallestHitFraction, MemoryAllocator& allocator) const; + /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* collider, MemoryAllocator& allocator) const override; @@ -125,6 +129,9 @@ class HeightFieldShape : public ConcaveShape { /// Compute the shape Id for a given triangle uint computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const; + /// Compute the first grid cell of the heightfield intersected by a ray + bool computeEnteringRayGridCoordinates(const Ray& ray, int& i, int& j, Vector3& outHitPoint) const; + /// Destructor virtual ~HeightFieldShape() override = default; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index f3693fc1d..9ac5f03be 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -27,6 +27,7 @@ #include #include #include +#include using namespace reactphysics3d; @@ -232,24 +233,98 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collide RP3D_PROFILE("HeightFieldShape::raycast()", mProfiler); - // Compute the AABB for the ray - const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); - const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd)); - - // Compute the triangles overlapping with the ray AABB - Array triangleVertices(allocator, 64); - Array triangleVerticesNormals(allocator, 64); - Array shapeIds(allocator, 64); - computeOverlappingTriangles(rayAABB, triangleVertices, triangleVerticesNormals, shapeIds, allocator); - - assert(triangleVertices.size() == triangleVerticesNormals.size()); - assert(shapeIds.size() == triangleVertices.size() / 3); - assert(triangleVertices.size() % 3 == 0); - assert(triangleVerticesNormals.size() % 3 == 0); + // Apply the concave mesh inverse scale factor because the mesh is stored without scaling + // inside the dynamic AABB tree + const Vector3 inverseScale(decimal(1.0) / mScale.x, decimal(1.0) / mScale.y, decimal(1.0) / mScale.z); + Ray scaledRay(ray.point1 * inverseScale, ray.point2 * inverseScale, ray.maxFraction); + + // Compute the grid coordinates where the ray is entering the AABB of the height field + int i, j; + Vector3 outHitGridPoint; + bool isIntersecting = computeEnteringRayGridCoordinates(scaledRay, i, j, outHitGridPoint); + assert(isIntersecting); + + const int nbCellsI = mNbColumns - 1; + const int nbCellsJ = mNbRows - 1; + + const Vector3 aabbSize = mAABB.getExtent(); + + const Vector3 rayDirection = scaledRay.point2 - scaledRay.point1; + + int stepI, stepJ; + decimal tMaxI, tMaxJ, nextI, nextJ, tDeltaI, tDeltaJ, sizeI, sizeJ; + + switch(mUpAxis) { + case 0 : stepI = rayDirection.y > 0 ? 1 : (rayDirection.y < 0 ? -1 : 0); + stepJ = rayDirection.z > 0 ? 1 : (rayDirection.z < 0 ? -1 : 0); + nextI = stepI >= 0 ? i + 1 : i; + nextJ = stepJ >= 0 ? j + 1 : j; + sizeI = aabbSize.y / nbCellsI; + sizeJ = aabbSize.z / nbCellsJ; + tMaxI = ((nextI * sizeI) - outHitGridPoint.y) / rayDirection.y; + tMaxJ = ((nextJ * sizeJ) - outHitGridPoint.z) / rayDirection.z; + tDeltaI = sizeI / std::abs(rayDirection.y); + tDeltaJ = sizeJ / std::abs(rayDirection.z); + break; + case 1 : stepI = rayDirection.x > 0 ? 1 : (rayDirection.x < 0 ? -1 : 0); + stepJ = rayDirection.z > 0 ? 1 : (rayDirection.z < 0 ? -1 : 0); + nextI = stepI >= 0 ? i + 1 : i; + nextJ = stepJ >= 0 ? j + 1 : j; + sizeI = aabbSize.x / nbCellsI; + sizeJ = aabbSize.z / nbCellsJ; + tMaxI = ((nextI * sizeI) - outHitGridPoint.x) / rayDirection.x; + tMaxJ = ((nextJ * sizeJ) - outHitGridPoint.z) / rayDirection.z; + tDeltaI = sizeI / std::abs(rayDirection.x); + tDeltaJ = sizeJ / std::abs(rayDirection.z); + break; + case 2 : stepI = rayDirection.x > 0 ? 1 : (rayDirection.x < 0 ? -1 : 0); + stepJ = rayDirection.y > 0 ? 1 : (rayDirection.y < 0 ? -1 : 0); + nextI = stepI >= 0 ? i + 1 : i; + nextJ = stepJ >= 0 ? j + 1 : j; + sizeI = aabbSize.x / nbCellsI; + sizeJ = aabbSize.y / nbCellsJ; + tMaxI = ((nextI * sizeI) - outHitGridPoint.x) / rayDirection.x; + tMaxJ = ((nextJ * sizeJ) - outHitGridPoint.y) / rayDirection.y; + tDeltaI = sizeI / std::abs(rayDirection.x); + tDeltaJ = sizeJ / std::abs(rayDirection.y); + break; + } bool isHit = false; decimal smallestHitFraction = ray.maxFraction; + while (i >= 0 && i < nbCellsI && j >= 0 && j < nbCellsJ) { + + // TODO : Remove this + //std::cout << "Cell " << i << ", " << j << std::endl; + + // Compute the four point of the current quad + const Vector3 p1 = getVertexAt(i, j); + const Vector3 p2 = getVertexAt(i, j + 1); + const Vector3 p3 = getVertexAt(i + 1, j); + const Vector3 p4 = getVertexAt(i + 1, j + 1); + + // Raycast against the first triangle of the cell + uint shapeId = computeTriangleShapeId(i, j, 0); + isHit |= raycastTriangle(ray, p1, p2, p3, shapeId, collider, raycastInfo, smallestHitFraction, allocator); + + // Raycast against the second triangle of the cell + shapeId = computeTriangleShapeId(i, j, 1); + isHit |= raycastTriangle(ray, p3, p2, p4, shapeId, collider, raycastInfo, smallestHitFraction, allocator); + + if (stepI == 0 && stepJ == 0) break; + + if (tMaxI < tMaxJ) { + tMaxI += tDeltaI; + i += stepI; + } + else { + tMaxJ += tDeltaJ; + j += stepJ; + } + } + + /* // For each overlapping triangle const uint32 nbShapeIds = shapeIds.size(); for (uint32 i=0; i < nbShapeIds; i++) @@ -287,10 +362,108 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collide isHit = true; } } + */ return isHit; } +// Raycast a single triangle of the height-field +bool HeightFieldShape::raycastTriangle(const Ray& ray, const Vector3& p1, const Vector3& p2, const Vector3& p3, uint shapeId, + Collider* collider, RaycastInfo& raycastInfo, decimal& smallestHitFraction, MemoryAllocator& allocator) const { + + // Generate the first triangle for the current grid rectangle + Vector3 triangleVertices[3] = {p1, p2, p3}; + + // Compute the triangle normal + Vector3 triangleNormal = (p2 - p1).cross(p3 - p1).getUnit(); + + // Use the triangle face normal as vertices normals (this is an aproximation. The correct + // solution would be to compute all the normals of the neighbor triangles and use their + // weighted average (with incident angle as weight) at the vertices. However, this solution + // seems too expensive (it requires to compute the normal of all neighbor triangles instead + // and compute the angle of incident edges with asin(). Maybe we could also precompute the + // vertices normal at the HeightFieldShape constructor but it will require extra memory to + // store them. + Vector3 triangleVerticesNormals[3] = {triangleNormal, triangleNormal, triangleNormal}; + + // Create a triangle collision shape + TriangleShape triangleShape(triangleVertices, triangleVerticesNormals, shapeId, mTriangleHalfEdgeStructure, allocator); + triangleShape.setRaycastTestType(getRaycastTestType()); + +#ifdef IS_RP3D_PROFILING_ENABLED + + + // Set the profiler to the triangle shape + triangleShape.setProfiler(mProfiler); + +#endif + + // Ray casting test against the collision shape + RaycastInfo triangleRaycastInfo; + bool isTriangleHit = triangleShape.raycast(ray, triangleRaycastInfo, collider, allocator); + + // If the ray hit the collision shape + if (isTriangleHit && triangleRaycastInfo.hitFraction <= smallestHitFraction) { + + assert(triangleRaycastInfo.hitFraction >= decimal(0.0)); + + raycastInfo.body = triangleRaycastInfo.body; + raycastInfo.collider = triangleRaycastInfo.collider; + raycastInfo.hitFraction = triangleRaycastInfo.hitFraction; + raycastInfo.worldPoint = triangleRaycastInfo.worldPoint; + raycastInfo.worldNormal = triangleRaycastInfo.worldNormal; + raycastInfo.meshSubpart = -1; + raycastInfo.triangleIndex = -1; + + smallestHitFraction = triangleRaycastInfo.hitFraction; + + return true; + } + + return false; +} + +// Compute the first grid cell of the heightfield intersected by a ray. +/// This method returns true if the ray hit the AABB of the height field and false otherwise +bool HeightFieldShape::computeEnteringRayGridCoordinates(const Ray& ray, int& i, int& j, Vector3& outHitGridPoint) const { + + decimal stepI, stepJ; + const Vector3 aabbSize = mAABB.getExtent(); + + const uint32 nbCellsI = mNbColumns - 1; + const uint32 nbCellsJ = mNbRows - 1; + + if (mAABB.raycast(ray, outHitGridPoint)) { + + // Map the hit point into the grid range [0, mNbColumns - 1], [0, mNbRows - 1] + outHitGridPoint -= mAABB.getMin(); + + switch(mUpAxis) { + case 0 : stepI = aabbSize.y / nbCellsI; + stepJ = aabbSize.z / nbCellsJ; + i = clamp(int(outHitGridPoint.y / stepI), 0, nbCellsI - 1); + j = clamp(int(outHitGridPoint.z / stepJ), 0, nbCellsJ - 1); + break; + case 1 : stepI = aabbSize.x / nbCellsI; + stepJ = aabbSize.z / nbCellsJ; + i = clamp(int(outHitGridPoint.x / stepI), 0, nbCellsI - 1); + j = clamp(int(outHitGridPoint.z / stepJ), 0, nbCellsJ - 1); + break; + case 2 : stepI = aabbSize.x / nbCellsI; + stepJ = aabbSize.y / nbCellsJ; + i = clamp(int(outHitGridPoint.x / stepI), 0, nbCellsI - 1); + j = clamp(int(outHitGridPoint.y / stepJ), 0, nbCellsJ - 1); + break; + } + + assert(i >= 0 && i < nbCellsI); + assert(j >= 0 && j < nbCellsJ); + return true; + } + + return false; +} + // Return the vertex (local-coordinates) of the height field at a given (x,y) position Vector3 HeightFieldShape::getVertexAt(int x, int y) const {