diff --git a/src/collision/shapes/ConvexShape.cpp b/src/collision/shapes/ConvexShape.cpp index 207b6804..878a3b17 100644 --- a/src/collision/shapes/ConvexShape.cpp +++ b/src/collision/shapes/ConvexShape.cpp @@ -48,12 +48,15 @@ Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction, // Get the support point without margin Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); - // Add the margin to the support point - Vector3 unitVec(0.0, -1.0, 0.0); - if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { - unitVec = direction.getUnit(); + if (mMargin != decimal(0.0)) { + + // Add the margin to the support point + Vector3 unitVec(0.0, -1.0, 0.0); + if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { + unitVec = direction.getUnit(); + } + supportPoint += unitVec * mMargin; } - supportPoint += unitVec * mMargin; return supportPoint; } diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 1b276b0d..304b013a 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -31,9 +31,11 @@ using namespace reactphysics3d; // Constructor // TODO : Add documentation to this constructor HeightFieldShape::HeightFieldShape(int width, int length, int minHeight, int maxHeight, - const void* heightFieldData, HeightDataType dataType, int upAxis) + const void* heightFieldData, HeightDataType dataType, int upAxis, + decimal integerHeightScale) : ConcaveShape(CONCAVE_MESH), mWidth(width), mLength(length), mMinHeight(minHeight), - mMaxHeight(maxHeight), mUpAxis(upAxis), mHeightDataType(dataType) { + mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale), + mHeightDataType(dataType) { assert(width >= 1); assert(length >= 1); @@ -41,6 +43,23 @@ HeightFieldShape::HeightFieldShape(int width, int length, int minHeight, int max assert(upAxis == 0 || upAxis == 1 || upAxis == 2); mHeightFieldData = heightFieldData; + + decimal halfHeight = (mMaxHeight - mMinHeight) * decimal(0.5); + assert(halfHeight > 0); + + // Compute the local AABB of the height field + if (mUpAxis == 0) { + mAABB.setMin(Vector3(-halfHeight, -mWidth * decimal(0.5), -mLength * decimal(0.5))); + mAABB.setMax(Vector3(halfHeight, mWidth * decimal(0.5), mLength* decimal(0.5))); + } + else if (mUpAxis == 1) { + mAABB.setMin(Vector3(-mWidth * decimal(0.5), -halfHeight, -mLength * decimal(0.5))); + mAABB.setMax(Vector3(mWidth * decimal(0.5), halfHeight, mLength * decimal(0.5))); + } + else if (mUpAxis == 2) { + mAABB.setMin(Vector3(-mWidth * decimal(0.5), -mLength * decimal(0.5), -halfHeight)); + mAABB.setMax(Vector3(mWidth * decimal(0.5), mLength * decimal(0.5), halfHeight)); + } } // Destructor @@ -48,10 +67,53 @@ HeightFieldShape::~HeightFieldShape() { } +// Return the local bounds of the shape in x, y and z directions. +// This method is used to compute the AABB of the box +/** + * @param min The minimum bounds of the shape in local-space coordinates + * @param max The maximum bounds of the shape in local-space coordinates + */ +void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { + min = mAABB.getMin() * mScaling; + max = mAABB.getMax() * mScaling; +} + // Use a callback method on all triangles of the concave shape inside a given AABB void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { - // TODO : Implement this + // Compute the non-scaled AABB + Vector3 inverseScaling(decimal(1.0) / mScaling.x, decimal(1.0) / mScaling.y, decimal(1.0) / mScaling.z); + AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling); + + // Compute the integer grid coordinates inside the area we need to test for collision + int minGridCoords[3]; + int maxGridCoords[3]; + computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, localAABB); + + +} + +// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and +// the AABB to collide +void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const { + + // Clamp the min/max coords of the AABB to collide inside the height field AABB + Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin()); + minPoint = Vector3::min(minPoint, mAABB.getMax()); + + Vector3 maxPoint = Vector3::max(aabbToCollide.getMax(), mAABB.getMin()); + maxPoint = Vector3::min(maxPoint, mAABB.getMax()); + + // 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) + minCoords[0] = computeIntegerGridValue(minPoint.x) - 1; + minCoords[1] = computeIntegerGridValue(minPoint.y) - 1; + minCoords[2] = computeIntegerGridValue(minPoint.z) - 1; + + maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1; + maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1; + maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1; } // Raycast method with feedback information diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index 9e0c3e07..f4a5d22d 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -66,12 +66,18 @@ class HeightFieldShape : public ConcaveShape { /// Up axis direction (0 => x, 1 => y, 2 => z) int mUpAxis; + /// Height values scale for height field with integer height values + decimal mIntegerHeightScale; + /// Data type of the height values HeightDataType mHeightDataType; /// Array of data with all the height values of the height field const void* mHeightFieldData; + /// Local AABB of the height field (without scaling) + AABB mAABB; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -94,11 +100,24 @@ class HeightFieldShape : public ConcaveShape { void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, Vector3* outTriangleVertices) const; + /// Return the vertex (local-coordinates) of the height field at a given (x,y) position + Vector3 getVertexAt(int x, int y) const; + + /// Return the height of a given (x,y) point in the height field + decimal getHeightAt(int x, int y) const; + + /// Return the closest inside integer grid value of a given floating grid value + int computeIntegerGridValue(decimal value) const; + + /// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide + void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const; + public: /// Constructor HeightFieldShape(int width, int length, int minHeight, int maxHeight, - const void* heightFieldData, HeightDataType dataType, int upAxis = 1); + const void* heightFieldData, HeightDataType dataType, + int upAxis = 1, decimal integerHeightScale = 1.0f); /// Destructor ~HeightFieldShape(); @@ -126,23 +145,40 @@ inline size_t HeightFieldShape::getSizeInBytes() const { return sizeof(HeightFieldShape); } -// Return the local bounds of the shape in x, y and z directions. -// This method is used to compute the AABB of the box -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -inline void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { - - // TODO : Implement this -} - // Set the local scaling vector of the collision shape inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) { - CollisionShape::setLocalScaling(scaling); +} - // TODO : Implement this +// Return the vertex (local-coordinates) of the height field at a given (x,y) position +inline Vector3 HeightFieldShape::getVertexAt(int x, int y) const { + + // Get the height value + const decimal height = getHeightAt(x, y); + + // Get the difference between the center of AABB and zero level of the height field + const decimal originToZeroHeight = (mMaxHeight - mMinHeight) * decimal(0.5); + + switch (mUpAxis) { + 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; + } +} + +// 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; + } +} + +// Return the closest inside integer grid value of a given floating grid value +inline int HeightFieldShape::computeIntegerGridValue(decimal value) const { + return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5); } // Return the local inertia tensor