diff --git a/src/collision/shapes/AABB.h b/src/collision/shapes/AABB.h index dfe9884c..cba31c70 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); + /// Inflate each side of the AABB by a given size + void inflate(decimal dx, decimal dy, decimal dz); + /// Return true if the current AABB is overlapping with the AABB in argument bool testCollision(const AABB& aabb) const; @@ -139,6 +142,12 @@ inline void AABB::setMax(const Vector3& max) { mMaxCoordinates = max; } +// 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); + mMinCoordinates -= Vector3(dx, dy, dz); +} + // Return true if the current AABB is overlapping with the AABB in argument. /// Two AABBs overlap if they overlap in the three x, y and z axis at the same time inline bool AABB::testCollision(const AABB& aabb) const { diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 927f69c2..ecf6f4a4 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -67,10 +67,6 @@ class BoxShape : public ConvexShape { /// Private assignment operator BoxShape& operator=(const BoxShape& shape); - /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const; @@ -143,17 +139,6 @@ inline size_t BoxShape::getSizeInBytes() const { return sizeof(BoxShape); } -// Return a local support point in a given direction with the object margin -inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { - - assert(mMargin > 0.0); - - return Vector3(direction.x < 0.0 ? -mExtent.x - mMargin : mExtent.x + mMargin, - direction.y < 0.0 ? -mExtent.y - mMargin : mExtent.y + mMargin, - direction.z < 0.0 ? -mExtent.z - mMargin : mExtent.z + mMargin); -} - // Return a local support point in a given direction without the objec margin inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const { diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index b5efbc5a..40d00609 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -37,7 +37,7 @@ using namespace reactphysics3d; * @param height The height of the capsule (in meters) */ CapsuleShape::CapsuleShape(decimal radius, decimal height) - : ConvexShape(CAPSULE, radius), mRadius(radius), mHalfHeight(height * decimal(0.5)) { + : ConvexShape(CAPSULE, radius), mHalfHeight(height * decimal(0.5)) { assert(radius > decimal(0.0)); assert(height > decimal(0.0)); } @@ -47,63 +47,6 @@ CapsuleShape::~CapsuleShape() { } -// Return a local support point in a given direction with the object margin. -/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d" -/// of the convex hull of a set of convex objects is the support point "p" in the set of all -/// support points from all the convex objects with the maximum dot product with the direction "d". -/// Therefore, in this method, we compute the support points of both top and bottom spheres of -/// the capsule and return the point with the maximum dot product with the direction vector. Note -/// that the object margin is implicitly the radius and height of the capsule. -Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { - - // If the direction vector is not the zero vector - if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) { - - Vector3 unitDirection = direction.getUnit(); - - // Support point top sphere - Vector3 centerTopSphere(0, mHalfHeight, 0); - Vector3 topSpherePoint = centerTopSphere + unitDirection * mRadius; - decimal dotProductTop = topSpherePoint.dot(direction); - - // Support point bottom sphere - Vector3 centerBottomSphere(0, -mHalfHeight, 0); - Vector3 bottomSpherePoint = centerBottomSphere + unitDirection * mRadius; - decimal dotProductBottom = bottomSpherePoint.dot(direction); - - // Return the point with the maximum dot product - if (dotProductTop > dotProductBottom) { - return topSpherePoint; - } - else { - return bottomSpherePoint; - } - } - - // If the direction vector is the zero vector we return a point on the - // boundary of the capsule - return Vector3(0, mRadius, 0); -} - -// Return a local support point in a given direction without the object margin. -Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { - - // If the dot product of the direction and the local Y axis (dotProduct = direction.y) - // is positive - if (direction.y > 0.0) { - - // Return the top sphere center point - return Vector3(0, mHalfHeight, 0); - } - else { - - // Return the bottom sphere center point - return Vector3(0, -mHalfHeight, 0); - } -} - // Return the local inertia tensor of the capsule /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space @@ -115,13 +58,13 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1 decimal height = mHalfHeight + mHalfHeight; - decimal radiusSquare = mRadius * mRadius; + decimal radiusSquare = mMargin * mMargin; decimal heightSquare = height * height; decimal radiusSquareDouble = radiusSquare + radiusSquare; - decimal factor1 = decimal(2.0) * mRadius / (decimal(4.0) * mRadius + decimal(3.0) * height); - decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mRadius + decimal(3.0) * height); + decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height); + decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height); decimal sum1 = decimal(0.4) * radiusSquareDouble; - decimal sum2 = decimal(0.75) * height * mRadius + decimal(0.5) * heightSquare; + decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare; decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare; decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3; decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble; @@ -137,7 +80,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight; const decimal xSquare = localPoint.x * localPoint.x; const decimal zSquare = localPoint.z * localPoint.z; - const decimal squareRadius = mRadius * mRadius; + const decimal squareRadius = mMargin * mMargin; // Return true if the point is inside the cylinder or one of the two spheres of the capsule return ((xSquare + zSquare) < squareRadius && @@ -163,16 +106,16 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* decimal dDotD = d.dot(d); // Test if the segment is outside the cylinder - decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mRadius, decimal(0.0))).dot(d); + decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mMargin, decimal(0.0))).dot(d); if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false; - decimal ddotDExtraCaps = decimal(2.0) * mRadius * d.y; + decimal ddotDExtraCaps = decimal(2.0) * mMargin * d.y; if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false; decimal nDotN = n.dot(n); decimal mDotN = m.dot(n); decimal a = dDotD * nDotN - nDotD * nDotD; - decimal k = m.dot(m) - mRadius * mRadius; + decimal k = m.dot(m) - mMargin * mMargin; decimal c = dDotD * k - mDotD * mDotD; // If the ray is parallel to the capsule axis @@ -298,7 +241,7 @@ bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& Vector3& hitLocalPoint, decimal& hitFraction) const { const Vector3 m = point1 - sphereCenter; - decimal c = m.dot(m) - mRadius * mRadius; + decimal c = m.dot(m) - mMargin * mMargin; // If the origin of the ray is inside the sphere, we return no intersection if (c < decimal(0.0)) return false; diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index efc38633..d0991689 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -50,9 +50,6 @@ class CapsuleShape : public ConvexShape { // -------------------- Attributes -------------------- // - /// Radius of the two spheres of the capsule - decimal mRadius; - /// Half height of the capsule (height = distance between the centers of the two spheres) decimal mHalfHeight; @@ -64,10 +61,6 @@ class CapsuleShape : public ConvexShape { /// Private assignment operator CapsuleShape& operator=(const CapsuleShape& shape); - /// Return a local support point in a given direction with the object margin. - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const; @@ -117,7 +110,7 @@ class CapsuleShape : public ConvexShape { * @return The radius of the capsule shape (in meters) */ inline decimal CapsuleShape::getRadius() const { - return mRadius; + return mMargin; } // Return the height of the capsule @@ -132,7 +125,7 @@ inline decimal CapsuleShape::getHeight() const { inline void CapsuleShape::setLocalScaling(const Vector3& scaling) { mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; + mMargin = (mMargin / mScaling.x) * scaling.x; CollisionShape::setLocalScaling(scaling); } @@ -151,16 +144,41 @@ inline size_t CapsuleShape::getSizeInBytes() const { inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { // Maximum bounds - max.x = mRadius; - max.y = mHalfHeight + mRadius; - max.z = mRadius; + max.x = mMargin; + max.y = mHalfHeight + mMargin; + max.z = mMargin; // Minimum bounds - min.x = -mRadius; + min.x = -mMargin; min.y = -max.y; min.z = min.x; } +// Return a local support point in a given direction without the object margin. +/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d" +/// of the convex hull of a set of convex objects is the support point "p" in the set of all +/// support points from all the convex objects with the maximum dot product with the direction "d". +/// Therefore, in this method, we compute the support points of both top and bottom spheres of +/// the capsule and return the point with the maximum dot product with the direction vector. Note +/// that the object margin is implicitly the radius and height of the capsule. +inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const { + + // Support point top sphere + decimal dotProductTop = mHalfHeight * direction.y; + + // Support point bottom sphere + decimal dotProductBottom = -mHalfHeight * direction.y; + + // Return the point with the maximum dot product + if (dotProductTop > dotProductBottom) { + return Vector3(0, mHalfHeight, 0); + } + else { + return Vector3(0, -mHalfHeight, 0); + } +} + } #endif diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index bd96b6d0..6edc012c 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -51,23 +51,6 @@ ConeShape::~ConeShape() { } -// Return a local support point in a given direction with the object margin -Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { - - // Compute the support point without the 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(); - } - supportPoint += unitVec * mMargin; - - return supportPoint; -} - // Return a local support point in a given direction without the object margin Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const { diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index 8fdcf6bd..06c7aa3d 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -72,10 +72,6 @@ class ConeShape : public ConvexShape { /// Private assignment operator ConeShape& operator=(const ConeShape& shape); - /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 35bf5bc1..2d66f953 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -142,24 +142,6 @@ ConvexMeshShape::~ConvexMeshShape() { } -// Return a local support point in a given direction with the object margin -Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { - - // Get the support point without the margin - Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); - - // Get the unit direction vector - Vector3 unitDirection = direction; - if (direction.lengthSquare() < MACHINE_EPSILON * MACHINE_EPSILON) { - unitDirection.setAllValues(1.0, 1.0, 1.0); - } - unitDirection.normalize(); - - // Add the margin to the support point and return it - return supportPoint + unitDirection * mMargin; -} - // Return a local support point in a given direction without the object margin. /// If the edges information is not used for collision detection, this method will go through /// the whole vertices list and pick up the vertex with the largest dot product in the support diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index cfdd22db..20a677ee 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -97,10 +97,6 @@ class ConvexMeshShape : public ConvexShape { /// Set the scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); - /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin. virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const; diff --git a/src/collision/shapes/ConvexShape.cpp b/src/collision/shapes/ConvexShape.cpp index 3ec13b2f..207b6804 100644 --- a/src/collision/shapes/ConvexShape.cpp +++ b/src/collision/shapes/ConvexShape.cpp @@ -40,3 +40,20 @@ ConvexShape::ConvexShape(CollisionShapeType type, decimal margin) ConvexShape::~ConvexShape() { } + +// Return a local support point in a given direction with the object margin +Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction, + void** cachedCollisionData) const { + + // 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(); + } + supportPoint += unitVec * mMargin; + + return supportPoint; +} diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index bc515e33..3aa0769a 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -55,8 +55,8 @@ class ConvexShape : public CollisionShape { ConvexShape& operator=(const ConvexShape& shape); // Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const=0; + Vector3 getLocalSupportPointWithMargin(const Vector3& direction, + void** cachedCollisionData) const; /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index 91fd8f40..1b256bc5 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/src/collision/shapes/CylinderShape.cpp @@ -48,23 +48,6 @@ CylinderShape::~CylinderShape() { } -// Return a local support point in a given direction with the object margin -Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { - - // Compute the support point without the margin - Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, NULL); - - // 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; - - return supportPoint; -} - // Return a local support point in a given direction without the object margin Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const { diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index 02dfa8c5..81f45f19 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -69,10 +69,6 @@ class CylinderShape : public ConvexShape { /// Private assignment operator CylinderShape& operator=(const CylinderShape& shape); - /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const; diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 27acc416..137d86a6 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -57,10 +57,6 @@ class SphereShape : public ConvexShape { /// Private assignment operator SphereShape& operator=(const SphereShape& shape); - /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const; @@ -121,22 +117,6 @@ inline size_t SphereShape::getSizeInBytes() const { return sizeof(SphereShape); } -// Return a local support point in a given direction with the object margin -inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { - - // If the direction vector is not the zero vector - if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) { - - // Return the support point of the sphere in the given direction - return mMargin * direction.getUnit(); - } - - // If the direction vector is the zero vector we return a point on the - // boundary of the sphere - return Vector3(0, mMargin, 0); -} - // Return a local support point in a given direction without the object margin inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const { diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 25532fd6..efeb3ca4 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -71,10 +71,6 @@ class TriangleShape : public ConvexShape { /// Private assignment operator TriangleShape& operator=(const TriangleShape& shape); - /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const; @@ -130,13 +126,6 @@ inline size_t TriangleShape::getSizeInBytes() const { return sizeof(TriangleShape); } -// Return a local support point in a given direction with the object margin -inline Vector3 TriangleShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { - - return getLocalSupportPointWithoutMargin(direction, cachedCollisionData); -} - // Return a local support point in a given direction without the object margin inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const { @@ -157,6 +146,9 @@ inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const { const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z); min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()); max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()); + + min -= Vector3(mMargin, mMargin, mMargin); + max += Vector3(mMargin, mMargin, mMargin); } // Set the local scaling vector of the collision shape diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h index 522f3ef9..6483c958 100644 --- a/test/tests/collision/TestAABB.h +++ b/test/tests/collision/TestAABB.h @@ -114,6 +114,16 @@ class TestAABB : public Test { test(aabb3.getMax().y == 98); test(aabb3.getMax().z == 76); + // -------- Test inflate() -------- // + AABB aabbInflate(Vector3(-3, 4, 8), Vector3(-1, 6, 32)); + aabbInflate.inflate(1, 2, 3); + test(approxEqual(aabbInflate.getMin().x, -4, 0.00001)); + test(approxEqual(aabbInflate.getMin().y, 2, 0.00001)); + test(approxEqual(aabbInflate.getMin().z, 5, 0.00001)); + test(approxEqual(aabbInflate.getMax().x, 0, 0.00001)); + test(approxEqual(aabbInflate.getMax().y, 8, 0.00001)); + test(approxEqual(aabbInflate.getMax().z, 35, 0.00001)); + // -------- Test getCenter() -------- // test(mAABB1.getCenter().x == 0);