From bacdf23f8ba4cb49a67b4c0ab3e82575f4ff5b85 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 31 Dec 2020 16:41:05 +0100 Subject: [PATCH] Fix issue and modifications in HeightFieldShape --- .../collision/shapes/TriangleShape.h | 6 + src/collision/shapes/HeightFieldShape.cpp | 203 +++++++----------- src/collision/shapes/TriangleShape.cpp | 37 +++- 3 files changed, 108 insertions(+), 138 deletions(-) diff --git a/include/reactphysics3d/collision/shapes/TriangleShape.h b/include/reactphysics3d/collision/shapes/TriangleShape.h index 7703a956..e2e783d5 100644 --- a/include/reactphysics3d/collision/shapes/TriangleShape.h +++ b/include/reactphysics3d/collision/shapes/TriangleShape.h @@ -110,6 +110,9 @@ class TriangleShape : public ConvexPolyhedronShape { TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, HalfEdgeStructure& triangleHalfEdgeStructure, MemoryAllocator& allocator); + /// Constructor + TriangleShape(const Vector3* vertices, uint shapeId, HalfEdgeStructure& triangleHalfEdgeStructure, MemoryAllocator& allocator); + /// Destructor virtual ~TriangleShape() override = default; @@ -262,6 +265,7 @@ RP3D_FORCE_INLINE Vector3 TriangleShape::getVertexPosition(uint vertexIndex) con // Return the normal vector of a given face of the polyhedron RP3D_FORCE_INLINE Vector3 TriangleShape::getFaceNormal(uint faceIndex) const { assert(faceIndex < 2); + assert(mNormal.length() > decimal(0.0)); return faceIndex == 0 ? mNormal : -mNormal; } @@ -311,6 +315,8 @@ RP3D_FORCE_INLINE decimal TriangleShape::getVolume() const { /// middle of the triangle, we return the true triangle normal. RP3D_FORCE_INLINE Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const { + assert(mNormal.length() > decimal(0.0)); + // Compute the barycentric coordinates of the point in the triangle decimal u, v, w; computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 9ac5f03b..22d76431 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -238,132 +238,93 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collide 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); + bool isHit = false; + // 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); + if (computeEnteringRayGridCoordinates(scaledRay, i, j, outHitGridPoint)) { - const int nbCellsI = mNbColumns - 1; - const int nbCellsJ = mNbRows - 1; + const int nbCellsI = mNbColumns - 1; + const int nbCellsJ = mNbRows - 1; - const Vector3 aabbSize = mAABB.getExtent(); + const Vector3 aabbSize = mAABB.getExtent(); - const Vector3 rayDirection = scaledRay.point2 - scaledRay.point1; + const Vector3 rayDirection = scaledRay.point2 - scaledRay.point1; - int stepI, stepJ; - decimal tMaxI, tMaxJ, nextI, nextJ, tDeltaI, tDeltaJ, sizeI, sizeJ; + 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; + 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; } - else { - tMaxJ += tDeltaJ; - j += stepJ; + + 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++) - { - // Create a triangle collision shape - TriangleShape triangleShape(&(triangleVertices[i * 3]), &(triangleVerticesNormals[i * 3]), shapeIds[i], 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; - isHit = true; - } - } - */ - return isHit; } @@ -374,20 +335,8 @@ bool HeightFieldShape::raycastTriangle(const Ray& ray, const Vector3& p1, 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 triangleShape(triangleVertices, shapeId, mTriangleHalfEdgeStructure, allocator); triangleShape.setRaycastTestType(getRaycastTestType()); #ifdef IS_RP3D_PROFILING_ENABLED diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index abafb92c..c072ad84 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -37,15 +37,6 @@ using namespace reactphysics3d; // Constructor -/** - * Do not use this constructor. It is supposed to be used internally only. - * Use a ConcaveMeshShape instead. - * @param point1 First point of the triangle - * @param point2 Second point of the triangle - * @param point3 Third point of the triangle - * @param verticesNormals The three vertices normals for smooth mesh collision - * @param margin The collision margin (in meters) around the collision shape - */ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, HalfEdgeStructure& triangleHalfEdgeStructure, MemoryAllocator& allocator) : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, allocator), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure) { @@ -66,6 +57,27 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor mId = shapeId; } +// Constructor for raycasting +TriangleShape::TriangleShape(const Vector3* vertices, uint shapeId, HalfEdgeStructure& triangleHalfEdgeStructure, MemoryAllocator& allocator) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, allocator), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure) { + + mPoints[0] = vertices[0]; + mPoints[1] = vertices[1]; + mPoints[2] = vertices[2]; + + // The normal is not used when creating the triangle shape with this constructor (for raycasting for instance) + mNormal = Vector3(0, 0, 0); + + // Interpolated normals are not used in this constructor (for raycasting for instance) + mVerticesNormals[0] = mNormal; + mVerticesNormals[1] = mNormal; + mVerticesNormals[2] = mNormal; + + mRaycastTestType = TriangleRaycastSide::FRONT; + + mId = shapeId; +} + // This method implements the technique described in Game Physics Pearl book // by Gino van der Bergen and Dirk Gregorius to get smooth triangle mesh collision. The idea is // to replace the contact normal of the triangle shape with the precomputed normal of the triangle @@ -182,13 +194,16 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false; - Vector3 localHitNormal = mNormal.dot(pq) > decimal(0.0) ? -mNormal : mNormal; + // Compute the triangle face normal + Vector3 normal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); + normal.normalize(); + normal = normal.dot(pq) > decimal(0.0) ? -normal : normal; raycastInfo.body = collider->getBody(); raycastInfo.collider = collider; raycastInfo.worldPoint = localHitPoint; raycastInfo.hitFraction = hitFraction; - raycastInfo.worldNormal = localHitNormal; + raycastInfo.worldNormal = normal; return true; }