mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix issue and modifications in HeightFieldShape

Browse Source
This commit is contained in:
Daniel Chappuis 2020-12-31 16:41:05 +01:00
parent a8d2478a7e
commit bacdf23f8b
3 changed files with 108 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/reactphysics3d/collision/shapes/TriangleShape.h
View File

@ -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);

203
src/collision/shapes/HeightFieldShape.cpp
View File

@ -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

37
src/collision/shapes/TriangleShape.cpp
View File

@ -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;
}