diff --git a/src/collision/shapes/AABB.h b/src/collision/shapes/AABB.h index cba31c70..e8ced67b 100644 --- a/src/collision/shapes/AABB.h +++ b/src/collision/shapes/AABB.h @@ -82,6 +82,9 @@ class AABB { /// Set the maximum coordinates of the AABB void setMax(const Vector3& max); + /// Return the size of the AABB in the three dimension x, y and z + Vector3 getExtent() const; + /// Inflate each side of the AABB by a given size void inflate(decimal dx, decimal dy, decimal dz); @@ -142,6 +145,11 @@ inline void AABB::setMax(const Vector3& max) { mMaxCoordinates = max; } +// Return the size of the AABB in the three dimension x, y and z +inline Vector3 AABB::getExtent() const { + return mMaxCoordinates - mMinCoordinates; +} + // Inflate each side of the AABB by a given size inline void AABB::inflate(decimal dx, decimal dy, decimal dz) { mMaxCoordinates += Vector3(dx, dy, dz); diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 304b013a..f2fb0b84 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -30,15 +30,18 @@ using namespace reactphysics3d; // Constructor // TODO : Add documentation to this constructor -HeightFieldShape::HeightFieldShape(int width, int length, int minHeight, int maxHeight, +HeightFieldShape::HeightFieldShape(int nbWidthGridPoints, int nbLengthGridPoints, decimal minHeight, decimal maxHeight, const void* heightFieldData, HeightDataType dataType, int upAxis, decimal integerHeightScale) - : ConcaveShape(CONCAVE_MESH), mWidth(width), mLength(length), mMinHeight(minHeight), + : ConcaveShape(CONCAVE_MESH), mNbWidthGridPoints(nbWidthGridPoints), mNbLengthGridPoints(nbLengthGridPoints), + mWidth(nbWidthGridPoints - 1), mLength(nbWidthGridPoints - 1), mMinHeight(minHeight), mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale), mHeightDataType(dataType) { - assert(width >= 1); - assert(length >= 1); + assert(nbWidthGridPoints >= 2); + assert(nbLengthGridPoints >= 2); + assert(mWidth >= 1); + assert(mLength >= 1); assert(minHeight <= maxHeight); assert(upAxis == 0 || upAxis == 1 || upAxis == 2); @@ -78,7 +81,10 @@ void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { max = mAABB.getMax() * mScaling; } -// Use a callback method on all triangles of the concave shape inside a given AABB +// Test collision with the triangles of the height field shape. The idea is to use the AABB +// of the body when need to test and see against which triangles of the height-field we need +// to test for collision. We compute the sub-grid points that are inside the other body's AABB +// and then for each rectangle in the sub-grid we generate two triangles that we use to test collision. void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { // Compute the non-scaled AABB @@ -90,7 +96,56 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& int maxGridCoords[3]; computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, localAABB); + // Compute the starting and ending coords of the sub-grid according to the up axis + int iMin, iMax, jMin, jMax; + switch(mUpAxis) { + case 0 : iMin = clamp(minGridCoords[1], 0, mNbWidthGridPoints - 1); + iMax = clamp(maxGridCoords[1], 0, mNbWidthGridPoints - 1); + jMin = clamp(minGridCoords[2], 0, mNbLengthGridPoints - 1); + jMax = clamp(maxGridCoords[2], 0, mNbLengthGridPoints - 1); + break; + case 1 : iMin = clamp(minGridCoords[0], 0, mNbWidthGridPoints - 1); + iMax = clamp(maxGridCoords[0], 0, mNbWidthGridPoints - 1); + jMin = clamp(minGridCoords[2], 0, mNbLengthGridPoints - 1); + jMax = clamp(maxGridCoords[2], 0, mNbLengthGridPoints - 1); + break; + case 2 : iMin = clamp(minGridCoords[0], 0, mNbWidthGridPoints - 1); + iMax = clamp(maxGridCoords[0], 0, mNbWidthGridPoints - 1); + jMin = clamp(minGridCoords[1], 0, mNbLengthGridPoints - 1); + jMax = clamp(maxGridCoords[1], 0, mNbLengthGridPoints - 1); + break; + } + assert(iMin >= 0 && iMin < mNbWidthGridPoints); + assert(iMax >= 0 && iMax < mNbWidthGridPoints); + assert(jMin >= 0 && jMin < mNbLengthGridPoints); + assert(jMax >= 0 && jMax < mNbLengthGridPoints); + + // For each sub-grid points (except the last ones one each dimension) + for (int i = iMin; i < iMax; i++) { + for (int j = jMin; j < jMax; j++) { + + // Compute the four point of the current quad + Vector3 p1 = getVertexAt(i, j); + Vector3 p2 = getVertexAt(i, j + 1); + Vector3 p3 = getVertexAt(i + 1, j); + Vector3 p4 = getVertexAt(i + 1, j + 1); + + // Generate the first triangle for the current grid rectangle + Vector3 trianglePoints[3] = {p1, p2, p3}; + + // Test collision against the first triangle + callback.testTriangle(trianglePoints); + + // Generate the second triangle for the current grid rectangle + trianglePoints[0] = p3; + trianglePoints[1] = p2; + trianglePoints[2] = p4; + + // Test collision against the second triangle + callback.testTriangle(trianglePoints); + } + } } // Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and @@ -104,6 +159,13 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor Vector3 maxPoint = Vector3::max(aabbToCollide.getMax(), mAABB.getMin()); maxPoint = Vector3::min(maxPoint, mAABB.getMax()); + // Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints] + // and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2] + // and [-mLength/2 ... mLength/2] + const Vector3 translateVec = mAABB.getExtent(); + minPoint += translateVec; + maxPoint += translateVec; + // Convert the floating min/max coords of the AABB into closest integer // grid values (note that we use the closest grid coordinate that is out // of the AABB) diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index f4a5d22d..9d6d250a 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -51,11 +51,17 @@ class HeightFieldShape : public ConcaveShape { // -------------------- Attributes -------------------- // + /// Number of grid points in the width dimension + int mNbWidthGridPoints; + + /// Number of grid points in the length dimension + int mNbLengthGridPoints; + /// Height field width - int mWidth; + decimal mWidth; /// Height field length - int mLength; + decimal mLength; /// Minimum height of the height field decimal mMinHeight; @@ -115,7 +121,7 @@ class HeightFieldShape : public ConcaveShape { public: /// Constructor - HeightFieldShape(int width, int length, int minHeight, int maxHeight, + HeightFieldShape(int nbWidthGridPoints, int nbLengthGridPoints, decimal minHeight, decimal maxHeight, const void* heightFieldData, HeightDataType dataType, int upAxis = 1, decimal integerHeightScale = 1.0f); @@ -163,16 +169,19 @@ inline Vector3 HeightFieldShape::getVertexAt(int x, int y) const { case 0: return Vector3(height - originToZeroHeight, -mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y) * mScaling; case 1: return Vector3(-mWidth * decimal(0.5) + x, height - originToZeroHeight, -mLength * decimal(0.5) + y) * mScaling; case 2: return Vector3(-mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y, height - originToZeroHeight) * mScaling; + default: assert(false); } + } // Return the height of a given (x,y) point in the height field inline decimal HeightFieldShape::getHeightAt(int x, int y) const { switch(mHeightDataType) { - case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mWidth + x]; - case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mWidth + x]; - case HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mWidth + x] * mIntegerHeightScale; + case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbWidthGridPoints + x]; + case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbWidthGridPoints + x]; + case HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mNbWidthGridPoints + x] * mIntegerHeightScale; + default: assert(false); } } diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 64dc2a61..a302bf62 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -46,6 +46,13 @@ inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) return (std::fabs(a - b) < epsilon); } +/// Function that returns the result of the "value" clamped by +/// two others values "lowerLimit" and "upperLimit" +inline int clamp(int value, int lowerLimit, int upperLimit) { + assert(lowerLimit <= upperLimit); + return std::min(std::max(value, lowerLimit), upperLimit); +} + /// Function that returns the result of the "value" clamped by /// two others values "lowerLimit" and "upperLimit" inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) { diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 66873111..83445c80 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -51,6 +51,7 @@ #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/ConvexMeshShape.h" #include "collision/shapes/ConcaveMeshShape.h" +#include "collision/shapes/HeightFieldShape.h" #include "collision/shapes/AABB.h" #include "collision/ProxyShape.h" #include "collision/RaycastInfo.h" diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h index 6483c958..2686cd72 100644 --- a/test/tests/collision/TestAABB.h +++ b/test/tests/collision/TestAABB.h @@ -124,6 +124,20 @@ class TestAABB : public Test { test(approxEqual(aabbInflate.getMax().y, 8, 0.00001)); test(approxEqual(aabbInflate.getMax().z, 35, 0.00001)); + // -------- Test getExtent() --------- // + + test(approxEqual(mAABB1.getExtent().x, 20)); + test(approxEqual(mAABB1.getExtent().y, 20)); + test(approxEqual(mAABB1.getExtent().z, 20)); + + test(approxEqual(mAABB2.getExtent().x, 3)); + test(approxEqual(mAABB2.getExtent().y, 16)); + test(approxEqual(mAABB2.getExtent().z, 60)); + + test(approxEqual(mAABB3.getExtent().x, 25)); + test(approxEqual(mAABB3.getExtent().y, 15)); + test(approxEqual(mAABB3.getExtent().z, 28)); + // -------- Test getCenter() -------- // test(mAABB1.getCenter().x == 0);