diff --git a/ephysics/collision/ContactManifold.hpp b/ephysics/collision/ContactManifold.hpp index 749cc6f..caae6bc 100644 --- a/ephysics/collision/ContactManifold.hpp +++ b/ephysics/collision/ContactManifold.hpp @@ -93,6 +93,7 @@ namespace ephysics { * only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, * this is only a guess that is faster to compute. This idea comes from the Bullet Physics library * by Erwin Coumans (http://wwww.bulletphysics.org). + */ int32_t getIndexToRemove(int32_t _indexMaxPenetration, const vec3& _newPoint) const; /// Remove a contact point from the manifold void removeContactPoint(uint32_t _index); diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index caabc5c..14843ba 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -93,7 +93,7 @@ void ConvexVsTriangleCallback::testTriangle(const vec3* _trianglePoints) { void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* _overlappingPair, etk::Vector _contactPoints, - NarrowPhaseCallback* narrowPhaseCallback) { + NarrowPhaseCallback* _callback) { // Set with the triangle vertices already processed to void further contacts with same triangle etk::Vector> processTriangleVertices; // Sort the list of narrow-phase contacts according to their penetration depth diff --git a/ephysics/collision/shapes/CollisionShape.hpp b/ephysics/collision/shapes/CollisionShape.hpp index 721b09d..007a7ee 100644 --- a/ephysics/collision/shapes/CollisionShape.hpp +++ b/ephysics/collision/shapes/CollisionShape.hpp @@ -35,6 +35,8 @@ class CollisionShape { CollisionShape(const CollisionShape& shape) = delete; /// DELETE assignment operator CollisionShape& operator=(const CollisionShape& shape) = delete; + /// Virtualize destructor + virtual ~CollisionShape() {}; /** * @brief Get the type of the collision shapes * @return The type of the collision shape (box, sphere, cylinder, ...) diff --git a/ephysics/collision/shapes/ConcaveShape.hpp b/ephysics/collision/shapes/ConcaveShape.hpp index 1cf75de..19eea6b 100644 --- a/ephysics/collision/shapes/ConcaveShape.hpp +++ b/ephysics/collision/shapes/ConcaveShape.hpp @@ -31,8 +31,6 @@ namespace ephysics { public : /// Constructor ConcaveShape(CollisionShapeType _type); - /// Destructor - virtual ~ConcaveShape(); /// DELETE copy-constructor ConcaveShape(const ConcaveShape& _shape) = delete; /// DELETE assignment operator diff --git a/ephysics/collision/shapes/ConvexMeshShape.cpp b/ephysics/collision/shapes/ConvexMeshShape.cpp index 8cae2e0..30f68fd 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.cpp +++ b/ephysics/collision/shapes/ConvexMeshShape.cpp @@ -11,23 +11,31 @@ using namespace ephysics; -ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, float margin) - : ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0), - m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { - assert(nbVertices > 0); - assert(stride > 0); - const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; +ConvexMeshShape::ConvexMeshShape(const float* _arrayVertices, + uint32_t _nbVertices, + int32_t _stride, + float _margin): + ConvexShape(CONVEX_MESH, _margin), + m_numberVertices(_nbVertices), + m_minBounds(0, 0, 0), + m_maxBounds(0, 0, 0), + m_isEdgesInformationUsed(false) { + assert(_nbVertices > 0); + assert(_stride > 0); + const unsigned char* vertexPointer = (const unsigned char*) _arrayVertices; // Copy all the vertices int32_to the int32_ternal array - for (uint32_t i=0; im_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo); +bool ConvexMeshShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const { + return _proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(_ray, _proxyShape, _raycastInfo); } -void ConvexMeshShape::setLocalScaling(const vec3& scaling) { - ConvexShape::setLocalScaling(scaling); +void ConvexMeshShape::setLocalScaling(const vec3& _scaling) { + ConvexShape::setLocalScaling(_scaling); recalculateBounds(); } @@ -171,73 +177,72 @@ size_t ConvexMeshShape::getSizeInBytes() const { return sizeof(ConvexMeshShape); } -void ConvexMeshShape::getLocalBounds(vec3& min, vec3& max) const { - min = m_minBounds; - max = m_maxBounds; +void ConvexMeshShape::getLocalBounds(vec3& _min, vec3& _max) const { + _min = m_minBounds; + _max = m_maxBounds; } -void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - float factor = (1.0f / float(3.0)) * mass; +void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { + float factor = (1.0f / float(3.0)) * _mass; vec3 realExtent = 0.5f * (m_maxBounds - m_minBounds); assert(realExtent.x() > 0 && realExtent.y() > 0 && realExtent.z() > 0); float xSquare = realExtent.x() * realExtent.x(); float ySquare = realExtent.y() * realExtent.y(); float zSquare = realExtent.z() * realExtent.z(); - tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0, - 0.0, factor * (xSquare + zSquare), 0.0, - 0.0, 0.0, factor * (xSquare + ySquare)); + _tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0, + 0.0, factor * (xSquare + zSquare), 0.0, + 0.0, 0.0, factor * (xSquare + ySquare)); } -void ConvexMeshShape::addVertex(const vec3& vertex) { +void ConvexMeshShape::addVertex(const vec3& _vertex) { // Add the vertex in to vertices array - m_vertices.pushBack(vertex); + m_vertices.pushBack(_vertex); m_numberVertices++; // Update the bounds of the mesh - if (vertex.x() * m_scaling.x() > m_maxBounds.x()) { - m_maxBounds.setX(vertex.x() * m_scaling.x()); + if (_vertex.x() * m_scaling.x() > m_maxBounds.x()) { + m_maxBounds.setX(_vertex.x() * m_scaling.x()); } - if (vertex.x() * m_scaling.x() < m_minBounds.x()) { - m_minBounds.setX(vertex.x() * m_scaling.x()); + if (_vertex.x() * m_scaling.x() < m_minBounds.x()) { + m_minBounds.setX(_vertex.x() * m_scaling.x()); } - if (vertex.y() * m_scaling.y() > m_maxBounds.y()) { - m_maxBounds.setY(vertex.y() * m_scaling.y()); + if (_vertex.y() * m_scaling.y() > m_maxBounds.y()) { + m_maxBounds.setY(_vertex.y() * m_scaling.y()); } - if (vertex.y() * m_scaling.y() < m_minBounds.y()) { - m_minBounds.setY(vertex.y() * m_scaling.y()); + if (_vertex.y() * m_scaling.y() < m_minBounds.y()) { + m_minBounds.setY(_vertex.y() * m_scaling.y()); } - if (vertex.z() * m_scaling.z() > m_maxBounds.z()) { - m_maxBounds.setZ(vertex.z() * m_scaling.z()); + if (_vertex.z() * m_scaling.z() > m_maxBounds.z()) { + m_maxBounds.setZ(_vertex.z() * m_scaling.z()); } - if (vertex.z() * m_scaling.z() < m_minBounds.z()) { - m_minBounds.setZ(vertex.z() * m_scaling.z()); + if (_vertex.z() * m_scaling.z() < m_minBounds.z()) { + m_minBounds.setZ(_vertex.z() * m_scaling.z()); } } -void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) { +void ConvexMeshShape::addEdge(uint32_t _v1, uint32_t _v2) { // If the entry for vertex v1 does not exist in the adjacency list - if (m_edgesAdjacencyList.count(v1) == 0) { - m_edgesAdjacencyList.add(v1, etk::Set()); + if (m_edgesAdjacencyList.count(_v1) == 0) { + m_edgesAdjacencyList.add(_v1, etk::Set()); } // If the entry for vertex v2 does not exist in the adjacency list - if (m_edgesAdjacencyList.count(v2) == 0) { - m_edgesAdjacencyList.add(v2, etk::Set()); + if (m_edgesAdjacencyList.count(_v2) == 0) { + m_edgesAdjacencyList.add(_v2, etk::Set()); } // Add the edge in the adjacency list - m_edgesAdjacencyList[v1].add(v2); - m_edgesAdjacencyList[v2].add(v1); + m_edgesAdjacencyList[_v1].add(_v2); + m_edgesAdjacencyList[_v2].add(_v1); } bool ConvexMeshShape::isEdgesInformationUsed() const { return m_isEdgesInformationUsed; } -void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { - m_isEdgesInformationUsed = isEdgesUsed; +void ConvexMeshShape::setIsEdgesInformationUsed(bool _isEdgesUsed) { + m_isEdgesInformationUsed = _isEdgesUsed; } -bool ConvexMeshShape::testPointInside(const vec3& localPoint, - ProxyShape* proxyShape) const { +bool ConvexMeshShape::testPointInside(const vec3& _localPoint, + ProxyShape* _proxyShape) const { // Use the GJK algorithm to test if the point is inside the convex mesh - return proxyShape->m_body->m_world.m_collisionDetection. - m_narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); + return _proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.testPointInside(_localPoint, _proxyShape); } \ No newline at end of file diff --git a/ephysics/collision/shapes/ConvexMeshShape.hpp b/ephysics/collision/shapes/ConvexMeshShape.hpp index fa99b2e..da764ee 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.hpp +++ b/ephysics/collision/shapes/ConvexMeshShape.hpp @@ -54,7 +54,7 @@ namespace ephysics { size_t getSizeInBytes() const override; public : /** - * @brief Constructor to initialize with an array of 3D vertices. + * @brief Constructor to initialize with an array of 3D vertices. * This method creates an int32_ternal copy of the input vertices. * @param[in] _arrayVertices Array with the vertices of the convex mesh * @param[in] _nbVertices Number of vertices in the convex mesh @@ -75,8 +75,12 @@ namespace ephysics { ConvexMeshShape(TriangleVertexArray* _triangleVertexArray, bool _isEdgesInformationUsed = true, float _margin = OBJECT_MARGIN); - /// Constructor. + /** + * @brief Constructor. + * If you use this constructor, you will need to set the vertices manually one by one using the addVertex() method. + */ ConvexMeshShape(float _margin = OBJECT_MARGIN); + public: void getLocalBounds(vec3& _min, vec3& _max) const override; void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override; /** @@ -86,7 +90,7 @@ namespace ephysics { void addVertex(const vec3& _vertex); /** * @brief Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. - * Note that the vertex indices start at zero and need to correspond to the order of + * @note that the vertex indices start at zero and need to correspond to the order of * the vertices in the vertices array in the constructor or the order of the calls * of the addVertex() methods that you use to add vertices int32_to the convex mesh. * @param[in] _v1 Index of the first vertex of the edge to add diff --git a/ephysics/collision/shapes/ConvexShape.hpp b/ephysics/collision/shapes/ConvexShape.hpp index bf4cf43..0b2f72a 100644 --- a/ephysics/collision/shapes/ConvexShape.hpp +++ b/ephysics/collision/shapes/ConvexShape.hpp @@ -14,23 +14,24 @@ namespace ephysics { * @brief It represents a convex collision shape associated with a * body that is used during the narrow-phase collision detection. */ -class ConvexShape : public CollisionShape { - protected : +class ConvexShape: public CollisionShape { + protected: float m_margin; //!< Margin used for the GJK collision detection algorithm /// Private copy-constructor - ConvexShape(const ConvexShape& shape) = delete; + ConvexShape(const ConvexShape& _shape) = delete; /// Private assignment operator - ConvexShape& operator=(const ConvexShape& shape) = delete; + ConvexShape& operator=(const ConvexShape& _shape) = delete; // Return a local support point in a given direction with the object margin virtual vec3 getLocalSupportPointWithMargin(const vec3& _direction, void** _cachedCollisionData) const; /// Return a local support point in a given direction without the object margin virtual vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const=0; bool testPointInside(const vec3& _worldPoint, ProxyShape* _proxyShape) const override = 0; - public : + public: /// Constructor - ConvexShape(CollisionShapeType type, float margin); + ConvexShape(CollisionShapeType _type, float _margin); /// Destructor virtual ~ConvexShape(); + public: /** * @brief Get the current object margin * @return The margin (in meters) around the collision shape diff --git a/ephysics/collision/shapes/CylinderShape.cpp b/ephysics/collision/shapes/CylinderShape.cpp index fc54f7a..bb28284 100644 --- a/ephysics/collision/shapes/CylinderShape.cpp +++ b/ephysics/collision/shapes/CylinderShape.cpp @@ -12,21 +12,16 @@ using namespace ephysics; -/** - * @param radius Radius of the cylinder (in meters) - * @param height Height of the cylinder (in meters) - * @param margin Collision margin (in meters) around the collision shape - */ -CylinderShape::CylinderShape(float radius, float height, float margin) - : ConvexShape(CYLINDER, margin), mRadius(radius), - m_halfHeight(height/float(2.0)) { - assert(radius > 0.0f); - assert(height > 0.0f); +CylinderShape::CylinderShape(float _radius, + float _height, + float _margin): + ConvexShape(CYLINDER, _margin), m_radius(_radius), m_halfHeight(_height/float(2.0)) { + assert(_radius > 0.0f); + assert(_height > 0.0f); } - -// Return a local support point in a given direction without the object margin -vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const { +vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, + void** _cachedCollisionData) const { vec3 supportPoint(0.0, 0.0, 0.0); float uDotv = _direction.y(); vec3 w(_direction.x(), 0.0, _direction.z()); @@ -37,7 +32,7 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, vo } else { supportPoint.setY(m_halfHeight); } - supportPoint += (mRadius / lengthW) * w; + supportPoint += (m_radius / lengthW) * w; } else { if (uDotv < 0.0) { supportPoint.setY(-m_halfHeight); @@ -48,234 +43,196 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, vo return supportPoint; } -// Raycast method with feedback information -/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by -/// Morgan Kaufmann. -bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - - const vec3 n = ray.point2 - ray.point1; - +bool CylinderShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const { + const vec3 n = _ray.point2 - _ray.point1; const float epsilon = float(0.01); vec3 p(float(0), -m_halfHeight, float(0)); vec3 q(float(0), m_halfHeight, float(0)); vec3 d = q - p; - vec3 m = ray.point1 - p; + vec3 m = _ray.point1 - p; float t; - float mDotD = m.dot(d); float nDotD = n.dot(d); float dDotD = d.dot(d); - // Test if the segment is outside the cylinder - if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) return false; - if (mDotD > dDotD && mDotD + nDotD > dDotD) return false; - + if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) { + return false; + } + if (mDotD > dDotD && mDotD + nDotD > dDotD) { + return false; + } float nDotN = n.dot(n); float mDotN = m.dot(n); - float a = dDotD * nDotN - nDotD * nDotD; - float k = m.dot(m) - mRadius * mRadius; + float k = m.dot(m) - m_radius * m_radius; float c = dDotD * k - mDotD * mDotD; - // If the ray is parallel to the cylinder axis if (etk::abs(a) < epsilon) { - // If the origin is outside the surface of the cylinder, we return no hit - if (c > 0.0f) return false; - - // Here we know that the segment int32_tersect an endcap of the cylinder - - // If the ray int32_tersects with the "p" endcap of the cylinder - if (mDotD < 0.0f) { - - t = -mDotN / nDotN; - - // If the int32_tersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < 0.0f || t > ray.maxFraction) return false; - - // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - vec3 normalDirection(0, float(-1), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder - - t = (nDotD - mDotN) / nDotN; - - // If the int32_tersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < 0.0f || t > ray.maxFraction) return false; - - // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - vec3 normalDirection(0, 1.0f, 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - else { // If the origin is inside the cylinder, we return no hit + if (c > 0.0f) { return false; } + // Here we know that the segment int32_tersect an endcap of the cylinder + // If the ray int32_tersects with the "p" endcap of the cylinder + if (mDotD < 0.0f) { + t = -mDotN / nDotN; + // If the int32_tersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } + // Compute the hit information + vec3 localHitPoint = _ray.point1 + t * n; + _raycastInfo.body = _proxyShape->getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; + vec3 normalDirection(0, float(-1), 0); + _raycastInfo.worldNormal = normalDirection; + return true; + } + // If the ray int32_tersects with the "q" endcap of the cylinder + if (mDotD > dDotD) { + t = (nDotD - mDotN) / nDotN; + // If the int32_tersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } + // Compute the hit information + vec3 localHitPoint = _ray.point1 + t * n; + _raycastInfo.body = _proxyShape->getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; + vec3 normalDirection(0, 1.0f, 0); + _raycastInfo.worldNormal = normalDirection; + return true; + } + // If the origin is inside the cylinder, we return no hit + return false; } float b = dDotD * mDotN - nDotD * mDotD; float discriminant = b * b - a * c; - // If the discriminant is negative, no real roots and therfore, no hit - if (discriminant < 0.0f) return false; - + if (discriminant < 0.0f) { + return false; + } // Compute the smallest root (first int32_tersection along the ray) float t0 = t = (-b - etk::sqrt(discriminant)) / a; - // If the int32_tersection is outside the cylinder on "p" endcap side float value = mDotD + t * nDotD; if (value < 0.0f) { - // If the ray is pointing away from the "p" endcap, we return no hit - if (nDotD <= 0.0f) return false; - + if (nDotD <= 0.0f) { + return false; + } // Compute the int32_tersection against the "p" endcap (int32_tersection agains whole plane) t = -mDotD / nDotD; - // Keep the int32_tersection if the it is inside the cylinder radius - if (k + t * (float(2.0) * mDotN + t) > 0.0f) return false; - + if (k + t * (float(2.0) * mDotN + t) > 0.0f) { + return false; + } // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < 0.0f || t > ray.maxFraction) return false; - + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; + vec3 localHitPoint = _ray.point1 + t * n; + _raycastInfo.body = _proxyShape->getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; vec3 normalDirection(0, float(-1.0), 0); - raycastInfo.worldNormal = normalDirection; - + _raycastInfo.worldNormal = normalDirection; return true; } - else if (value > dDotD) { // If the int32_tersection is outside the cylinder on the "q" side - + // If the int32_tersection is outside the cylinder on the "q" side + if (value > dDotD) { // If the ray is pointing away from the "q" endcap, we return no hit - if (nDotD >= 0.0f) return false; - + if (nDotD >= 0.0f) { + return false; + } // Compute the int32_tersection against the "q" endcap (int32_tersection against whole plane) t = (dDotD - mDotD) / nDotD; - // Keep the int32_tersection if it is inside the cylinder radius - if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) > - 0.0f) return false; - + if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) > 0.0f) { + return false; + } // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < 0.0f || t > ray.maxFraction) return false; - + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; + vec3 localHitPoint = _ray.point1 + t * n; + _raycastInfo.body = _proxyShape->getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; vec3 normalDirection(0, 1.0f, 0); - raycastInfo.worldNormal = normalDirection; - + _raycastInfo.worldNormal = normalDirection; return true; } - t = t0; - // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < 0.0f || t > ray.maxFraction) return false; - + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; + vec3 localHitPoint = _ray.point1 + t * n; + _raycastInfo.body = _proxyShape->getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; vec3 v = localHitPoint - p; vec3 w = (v.dot(d) / d.length2()) * d; vec3 normalDirection = (localHitPoint - (p + w)); - raycastInfo.worldNormal = normalDirection; - + _raycastInfo.worldNormal = normalDirection; return true; } -// Return the radius -/** - * @return Radius of the cylinder (in meters) - */ float CylinderShape::getRadius() const { - return mRadius; + return m_radius; } -// Return the height -/** - * @return Height of the cylinder (in meters) - */ float CylinderShape::getHeight() const { return m_halfHeight + m_halfHeight; } -// Set the scaling vector of the collision shape -void CylinderShape::setLocalScaling(const vec3& scaling) { - - m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); - mRadius = (mRadius / m_scaling.x()) * scaling.x(); - - CollisionShape::setLocalScaling(scaling); +void CylinderShape::setLocalScaling(const vec3& _scaling) { + m_halfHeight = (m_halfHeight / m_scaling.y()) * _scaling.y(); + m_radius = (m_radius / m_scaling.x()) * _scaling.x(); + CollisionShape::setLocalScaling(_scaling); } -// Return the number of bytes used by the collision shape size_t CylinderShape::getSizeInBytes() const { return sizeof(CylinderShape); } -// Return the local bounds of the shape in x, y and z directions -/** - * @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 CylinderShape::getLocalBounds(vec3& min, vec3& max) const { +void CylinderShape::getLocalBounds(vec3& _min, vec3& _max) const { // Maximum bounds - max.setX(mRadius + m_margin); - max.setY(m_halfHeight + m_margin); - max.setZ(max.x()); + _max.setX(m_radius + m_margin); + _max.setY(m_halfHeight + m_margin); + _max.setZ(_max.x()); // Minimum bounds - min.setX(-max.x()); - min.setY(-max.y()); - min.setZ(min.x()); + _min.setX(-_max.x()); + _min.setY(-_max.y()); + _min.setZ(_min.x()); } -// Return the local inertia tensor of the cylinder -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { +void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { float height = float(2.0) * m_halfHeight; - float diag = (1.0f / float(12.0)) * mass * (3 * mRadius * mRadius + height * height); - tensor.setValue(diag, 0.0, 0.0, 0.0, - 0.5f * mass * mRadius * mRadius, 0.0, - 0.0, 0.0, diag); + float diag = (1.0f / float(12.0)) * _mass * (3 * m_radius * m_radius + height * height); + _tensor.setValue(diag, 0.0, 0.0, 0.0, + 0.5f * _mass * m_radius * m_radius, 0.0, + 0.0, 0.0, diag); } -// Return true if a point is inside the collision shape -bool CylinderShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const{ - return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < mRadius * mRadius - && localPoint.y() < m_halfHeight - && localPoint.y() > -m_halfHeight); +bool CylinderShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const{ + return ( (_localPoint.x() * _localPoint.x() + _localPoint.z() * _localPoint.z()) < m_radius * m_radius + && _localPoint.y() < m_halfHeight + && _localPoint.y() > -m_halfHeight); } diff --git a/ephysics/collision/shapes/CylinderShape.hpp b/ephysics/collision/shapes/CylinderShape.hpp index f08637d..11ae1cf 100644 --- a/ephysics/collision/shapes/CylinderShape.hpp +++ b/ephysics/collision/shapes/CylinderShape.hpp @@ -13,42 +13,53 @@ #include namespace ephysics { -/** - * @brief It represents a cylinder collision shape around the Y axis - * and centered at the origin. The cylinder is defined by its height - * and the radius of its base. The "transform" of the corresponding - * rigid body gives an orientation and a position to the cylinder. - * This collision shape uses an extra margin distance around it for collision - * detection purpose. The default margin is 4cm (if your units are meters, - * which is recommended). In case, you want to simulate small objects - * (smaller than the margin distance), you might want to reduce the margin by - * specifying your own margin distance using the "margin" parameter in the - * constructor of the cylinder shape. Otherwise, it is recommended to use the - * default margin distance by not using the "margin" parameter in the constructor. - */ -class CylinderShape : public ConvexShape { - protected : - float mRadius; //!< Radius of the base - float m_halfHeight; //!< Half height of the cylinder - /// Private copy-constructor - CylinderShape(const CylinderShape&) = delete; - /// Private assignment operator - CylinderShape& operator=(const CylinderShape&) = delete; - vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override; - bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override; - bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override; - size_t getSizeInBytes() const override; - public : - /// Constructor - CylinderShape(float _radius, float _height, float _margin = OBJECT_MARGIN); - /// Return the radius - float getRadius() const; - /// Return the height - float getHeight() const; - void setLocalScaling(const vec3& _scaling) override; - void getLocalBounds(vec3& _min, vec3& _max) const override; - void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override; -}; + /** + * @brief It represents a cylinder collision shape around the Y axis + * and centered at the origin. The cylinder is defined by its height + * and the radius of its base. The "transform" of the corresponding + * rigid body gives an orientation and a position to the cylinder. + * This collision shape uses an extra margin distance around it for collision + * detection purpose. The default margin is 4cm (if your units are meters, + * which is recommended). In case, you want to simulate small objects + * (smaller than the margin distance), you might want to reduce the margin by + * specifying your own margin distance using the "margin" parameter in the + * constructor of the cylinder shape. Otherwise, it is recommended to use the + * default margin distance by not using the "margin" parameter in the constructor. + */ + class CylinderShape: public ConvexShape { + protected: + float m_radius; //!< Radius of the base + float m_halfHeight; //!< Half height of the cylinder + /// DELETED copy-constructor + CylinderShape(const CylinderShape&) = delete; + /// DELETED assignment operator + CylinderShape& operator=(const CylinderShape&) = delete; + vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override; + bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override; + bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override; + size_t getSizeInBytes() const override; + public: + /** + * @brief Contructor + * @param radius Radius of the cylinder (in meters) + * @param height Height of the cylinder (in meters) + * @param margin Collision margin (in meters) around the collision shape + */ + CylinderShape(float _radius, float _height, float _margin = OBJECT_MARGIN); + /** + * @breif Get the Shape radius + * @return Radius of the cylinder (in meters) + */ + float getRadius() const; + /** + * @breif Get the Shape height + * @return Height of the cylinder (in meters) + */ + float getHeight() const; + void setLocalScaling(const vec3& _scaling) override; + void getLocalBounds(vec3& _min, vec3& _max) const override; + void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override; + }; } diff --git a/ephysics/collision/shapes/HeightFieldShape.cpp b/ephysics/collision/shapes/HeightFieldShape.cpp index 10e5819..2eb208d 100644 --- a/ephysics/collision/shapes/HeightFieldShape.cpp +++ b/ephysics/collision/shapes/HeightFieldShape.cpp @@ -8,228 +8,183 @@ */ #include +// TODO: REMOVE this... using namespace ephysics; -/** - * @param nbGridColumns Number of columns in the grid of the height field - * @param nbGridRows Number of rows in the grid of the height field - * @param minHeight Minimum height value of the height field - * @param maxHeight Maximum height value of the height field - * @param heightFieldData Pointer to the first height value data (note that values are shared and not copied) - * @param dataType Data type for the height values (int32_t, float, double) - * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z) - * @param int32_tegerHeightScale Scaling factor used to scale the height values (only when height values type is int32_teger) - */ -HeightFieldShape::HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight, - const void* heightFieldData, HeightDataType dataType, int32_t upAxis, - float int32_tegerHeightScale) - : ConcaveShape(HEIGHTFIELD), m_numberColumns(nbGridColumns), m_numberRows(nbGridRows), - m_width(nbGridColumns - 1), m_length(nbGridRows - 1), m_minHeight(minHeight), - m_maxHeight(maxHeight), m_upAxis(upAxis), m_integerHeightScale(int32_tegerHeightScale), - m_heightDataType(dataType) { - - assert(nbGridColumns >= 2); - assert(nbGridRows >= 2); +HeightFieldShape::HeightFieldShape(int32_t _nbGridColumns, + int32_t _nbGridRows, + float _minHeight, + float _maxHeight, + const void* _heightFieldData, + HeightDataType _dataType, + int32_t _upAxis, + float _integerHeightScale): + ConcaveShape(HEIGHTFIELD), + m_numberColumns(_nbGridColumns), + m_numberRows(_nbGridRows), + m_width(_nbGridColumns - 1), + m_length(_nbGridRows - 1), + m_minHeight(_minHeight), + m_maxHeight(_maxHeight), + m_upAxis(_upAxis), + m_integerHeightScale(_integerHeightScale), + m_heightDataType(_dataType) { + assert(_nbGridColumns >= 2); + assert(_nbGridRows >= 2); assert(m_width >= 1); assert(m_length >= 1); - assert(minHeight <= maxHeight); - assert(upAxis == 0 || upAxis == 1 || upAxis == 2); - - m_heightFieldData = heightFieldData; - + assert(_minHeight <= _maxHeight); + assert(_upAxis == 0 || _upAxis == 1 || _upAxis == 2); + m_heightFieldData = _heightFieldData; float halfHeight = (m_maxHeight - m_minHeight) * 0.5f; assert(halfHeight >= 0); - // Compute the local AABB of the height field if (m_upAxis == 0) { m_AABB.setMin(vec3(-halfHeight, -m_width * 0.5f, -m_length * float(0.5))); m_AABB.setMax(vec3(halfHeight, m_width * 0.5f, m_length* float(0.5))); - } - else if (m_upAxis == 1) { + } else if (m_upAxis == 1) { m_AABB.setMin(vec3(-m_width * 0.5f, -halfHeight, -m_length * float(0.5))); m_AABB.setMax(vec3(m_width * 0.5f, halfHeight, m_length * float(0.5))); - } - else if (m_upAxis == 2) { + } else if (m_upAxis == 2) { m_AABB.setMin(vec3(-m_width * 0.5f, -m_length * float(0.5), -halfHeight)); m_AABB.setMax(vec3(m_width * 0.5f, m_length * float(0.5), halfHeight)); } } - -// 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(vec3& min, vec3& max) const { - min = m_AABB.getMin() * m_scaling; - max = m_AABB.getMax() * m_scaling; +void HeightFieldShape::getLocalBounds(vec3& _min, vec3& _max) const { + _min = m_AABB.getMin() * m_scaling; + _max = m_AABB.getMax() * m_scaling; } -// 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 - vec3 inverseScaling(1.0f / m_scaling.x(), 1.0f / m_scaling.y(), float(1.0) / m_scaling.z()); - AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling); - - // Compute the int32_teger grid coordinates inside the area we need to test for collision - int32_t minGridCoords[3]; - int32_t maxGridCoords[3]; - computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb); - - // Compute the starting and ending coords of the sub-grid according to the up axis - int32_t iMin = 0; - int32_t iMax = 0; - int32_t jMin = 0; - int32_t jMax = 0; - switch(m_upAxis) { - case 0 : iMin = clamp(minGridCoords[1], 0, m_numberColumns - 1); - iMax = clamp(maxGridCoords[1], 0, m_numberColumns - 1); - jMin = clamp(minGridCoords[2], 0, m_numberRows - 1); - jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1); - break; - case 1 : iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1); - iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1); - jMin = clamp(minGridCoords[2], 0, m_numberRows - 1); - jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1); - break; - case 2 : iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1); - iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1); - jMin = clamp(minGridCoords[1], 0, m_numberRows - 1); - jMax = clamp(maxGridCoords[1], 0, m_numberRows - 1); - break; - } - - assert(iMin >= 0 && iMin < m_numberColumns); - assert(iMax >= 0 && iMax < m_numberColumns); - assert(jMin >= 0 && jMin < m_numberRows); - assert(jMax >= 0 && jMax < m_numberRows); - - // For each sub-grid points (except the last ones one each dimension) - for (int32_t i = iMin; i < iMax; i++) { - for (int32_t j = jMin; j < jMax; j++) { - - // Compute the four point of the current quad - vec3 p1 = getVertexAt(i, j); - vec3 p2 = getVertexAt(i, j + 1); - vec3 p3 = getVertexAt(i + 1, j); - vec3 p4 = getVertexAt(i + 1, j + 1); - - // Generate the first triangle for the current grid rectangle - vec3 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); - } - } +void HeightFieldShape::testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const { + // Compute the non-scaled AABB + vec3 inverseScaling(1.0f / m_scaling.x(), 1.0f / m_scaling.y(), float(1.0) / m_scaling.z()); + AABB aabb(_localAABB.getMin() * inverseScaling, _localAABB.getMax() * inverseScaling); + // Compute the int32_teger grid coordinates inside the area we need to test for collision + int32_t minGridCoords[3]; + int32_t maxGridCoords[3]; + computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb); + // Compute the starting and ending coords of the sub-grid according to the up axis + int32_t iMin = 0; + int32_t iMax = 0; + int32_t jMin = 0; + int32_t jMax = 0; + switch(m_upAxis) { + case 0 : + iMin = clamp(minGridCoords[1], 0, m_numberColumns - 1); + iMax = clamp(maxGridCoords[1], 0, m_numberColumns - 1); + jMin = clamp(minGridCoords[2], 0, m_numberRows - 1); + jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1); + break; + case 1 : + iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1); + iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1); + jMin = clamp(minGridCoords[2], 0, m_numberRows - 1); + jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1); + break; + case 2 : + iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1); + iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1); + jMin = clamp(minGridCoords[1], 0, m_numberRows - 1); + jMax = clamp(maxGridCoords[1], 0, m_numberRows - 1); + break; + } + assert(iMin >= 0 && iMin < m_numberColumns); + assert(iMax >= 0 && iMax < m_numberColumns); + assert(jMin >= 0 && jMin < m_numberRows); + assert(jMax >= 0 && jMax < m_numberRows); + // For each sub-grid points (except the last ones one each dimension) + for (int32_t i = iMin; i < iMax; i++) { + for (int32_t j = jMin; j < jMax; j++) { + // Compute the four point of the current quad + vec3 p1 = getVertexAt(i, j); + vec3 p2 = getVertexAt(i, j + 1); + vec3 p3 = getVertexAt(i + 1, j); + vec3 p4 = getVertexAt(i + 1, j + 1); + // Generate the first triangle for the current grid rectangle + vec3 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 int32_tersection of the AABB of the height field and -// the AABB to collide -void HeightFieldShape::computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const { - +void HeightFieldShape::computeMinMaxGridCoordinates(int32_t* _minCoords, int32_t* _maxCoords, const AABB& _aabbToCollide) const { // Clamp the min/max coords of the AABB to collide inside the height field AABB - vec3 minPoint = etk::max(aabbToCollide.getMin(), m_AABB.getMin()); + vec3 minPoint = etk::max(_aabbToCollide.getMin(), m_AABB.getMin()); minPoint = etk::min(minPoint, m_AABB.getMax()); - - vec3 maxPoint = etk::min(aabbToCollide.getMax(), m_AABB.getMax()); + vec3 maxPoint = etk::min(_aabbToCollide.getMax(), m_AABB.getMax()); maxPoint = etk::max(maxPoint, m_AABB.getMin()); - // 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 ... m_width/2] // and [-m_length/2 ... m_length/2] const vec3 translateVec = m_AABB.getExtent() * 0.5f; minPoint += translateVec; maxPoint += translateVec; - // Convert the floating min/max coords of the AABB int32_to closest int32_teger // 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; + _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 -/// Note that only the first triangle hit by the ray in the mesh will be returned, even if -/// the ray hits many triangles. -bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - +bool HeightFieldShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const { // TODO : Implement raycasting without using an AABB for the ray // but using a dynamic AABB tree or octree instead - PROFILE("HeightFieldShape::raycast()"); - - TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); - + TriangleOverlapCallback triangleCallback(_ray, _proxyShape, _raycastInfo, *this); // Compute the AABB for the ray - const vec3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); - const AABB rayAABB(etk::min(ray.point1, rayEnd), etk::max(ray.point1, rayEnd)); - + const vec3 rayEnd = _ray.point1 + _ray.maxFraction * (_ray.point2 - _ray.point1); + const AABB rayAABB(etk::min(_ray.point1, rayEnd), etk::max(_ray.point1, rayEnd)); testAllTriangles(triangleCallback, rayAABB); - return triangleCallback.getIsHit(); } -// Return the vertex (local-coordinates) of the height field at a given (x,y) position -vec3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const { - +vec3 HeightFieldShape::getVertexAt(int32_t _xxx, int32_t _yyy) const { // Get the height value - const float height = getHeightAt(x, y); - + const float height = getHeightAt(_xxx, _yyy); // Height values origin const float heightOrigin = -(m_maxHeight - m_minHeight) * 0.5f - m_minHeight; - vec3 vertex; switch (m_upAxis) { - case 0: vertex = vec3(heightOrigin + height, -m_width * 0.5f + x, -m_length * float(0.5) + y); - break; - case 1: vertex = vec3(-m_width * 0.5f + x, heightOrigin + height, -m_length * float(0.5) + y); - break; - case 2: vertex = vec3(-m_width * 0.5f + x, -m_length * float(0.5) + y, heightOrigin + height); - break; - default: assert(false); + case 0: + vertex = vec3(heightOrigin + height, -m_width * 0.5f + _xxx, -m_length * float(0.5) + _yyy); + break; + case 1: + vertex = vec3(-m_width * 0.5f + _xxx, heightOrigin + height, -m_length * float(0.5) + _yyy); + break; + case 2: + vertex = vec3(-m_width * 0.5f + _xxx, -m_length * float(0.5) + _yyy, heightOrigin + height); + break; + default: + assert(false); } - assert(m_AABB.contains(vertex)); - return vertex * m_scaling; } -// Raycast test between a ray and a triangle of the heightfield -void TriangleOverlapCallback::testTriangle(const vec3* trianglePoints) { - +void TriangleOverlapCallback::testTriangle(const vec3* _trianglePoints) { // Create a triangle collision shape float margin = m_heightFieldShape.getTriangleMargin(); - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + TriangleShape triangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin); triangleShape.setRaycastTestType(m_heightFieldShape.getRaycastTestType()); - // Ray casting test against the collision shape RaycastInfo raycastInfo; bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); - // If the ray hit the collision shape - if (isTriangleHit && raycastInfo.hitFraction <= m_smallestHitFraction) { - + if ( isTriangleHit + && raycastInfo.hitFraction <= m_smallestHitFraction) { assert(raycastInfo.hitFraction >= 0.0f); - m_raycastInfo.body = raycastInfo.body; m_raycastInfo.proxyShape = raycastInfo.proxyShape; m_raycastInfo.hitFraction = raycastInfo.hitFraction; @@ -237,66 +192,55 @@ void TriangleOverlapCallback::testTriangle(const vec3* trianglePoints) { m_raycastInfo.worldNormal = raycastInfo.worldNormal; m_raycastInfo.meshSubpart = -1; m_raycastInfo.triangleIndex = -1; - m_smallestHitFraction = raycastInfo.hitFraction; m_isHit = true; } } -// Return the number of rows in the height field int32_t HeightFieldShape::getNbRows() const { return m_numberRows; } -// Return the number of columns in the height field int32_t HeightFieldShape::getNbColumns() const { return m_numberColumns; } -// Return the type of height value in the height field HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const { return m_heightDataType; } -// Return the number of bytes used by the collision shape size_t HeightFieldShape::getSizeInBytes() const { return sizeof(HeightFieldShape); } -// Set the local scaling vector of the collision shape -void HeightFieldShape::setLocalScaling(const vec3& scaling) { - CollisionShape::setLocalScaling(scaling); +void HeightFieldShape::setLocalScaling(const vec3& _scaling) { + CollisionShape::setLocalScaling(_scaling); } -// Return the height of a given (x,y) point in the height field -float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const { - +float HeightFieldShape::getHeightAt(int32_t _xxx, int32_t _yyy) const { switch(m_heightDataType) { - case HEIGHT_FLOAT_TYPE : return ((float*)m_heightFieldData)[y * m_numberColumns + x]; - case HEIGHT_DOUBLE_TYPE : return ((double*)m_heightFieldData)[y * m_numberColumns + x]; - case HEIGHT_INT_TYPE : return ((int32_t*)m_heightFieldData)[y * m_numberColumns + x] * m_integerHeightScale; - default: assert(false); return 0; + case HEIGHT_FLOAT_TYPE: + return ((float*)m_heightFieldData)[_yyy * m_numberColumns + _xxx]; + case HEIGHT_DOUBLE_TYPE: + return ((double*)m_heightFieldData)[_yyy * m_numberColumns + _xxx]; + case HEIGHT_INT_TYPE: + return ((int32_t*)m_heightFieldData)[_yyy * m_numberColumns + _xxx] * m_integerHeightScale; + default: + assert(false); + return 0; } } -// Return the closest inside int32_teger grid value of a given floating grid value -int32_t HeightFieldShape::computeIntegerGridValue(float value) const { - return (value < 0.0f) ? value - 0.5f : value + float(0.5); +int32_t HeightFieldShape::computeIntegerGridValue(float _value) const { + return (_value < 0.0f) ? _value - 0.5f : _value + 0.5f; } -// Return the local inertia tensor -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - +void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { // Default inertia tensor // Note that this is not very realistic for a concave triangle mesh. // However, in most cases, it will only be used static bodies and therefore, // the inertia tensor is not used. - tensor.setValue(mass, 0, 0, - 0, mass, 0, - 0, 0, mass); + _tensor.setValue(_mass, 0, 0, + 0, _mass, 0, + 0, 0, _mass); } diff --git a/ephysics/collision/shapes/HeightFieldShape.hpp b/ephysics/collision/shapes/HeightFieldShape.hpp index 340486d..f2e7309 100644 --- a/ephysics/collision/shapes/HeightFieldShape.hpp +++ b/ephysics/collision/shapes/HeightFieldShape.hpp @@ -77,9 +77,9 @@ namespace ephysics { HeightDataType m_heightDataType; //!< Data type of the height values const void* m_heightFieldData; //!< Array of data with all the height values of the height field AABB m_AABB; //!< Local AABB of the height field (without scaling) - /// Private copy-constructor + /// DELETED copy-constructor HeightFieldShape(const HeightFieldShape&) = delete; - /// Private assignment operator + /// DELETED assignment operator HeightFieldShape& operator=(const HeightFieldShape&) = delete; bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override; size_t getSizeInBytes() const override; @@ -99,7 +99,17 @@ namespace ephysics { /// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and the AABB to collide void computeMinMaxGridCoordinates(int32_t* _minCoords, int32_t* _maxCoords, const AABB& _aabbToCollide) const; public: - /// Constructor + /** + * @brief Contructor + * @param nbGridColumns Number of columns in the grid of the height field + * @param nbGridRows Number of rows in the grid of the height field + * @param minHeight Minimum height value of the height field + * @param maxHeight Maximum height value of the height field + * @param heightFieldData Pointer to the first height value data (note that values are shared and not copied) + * @param dataType Data type for the height values (int32_t, float, double) + * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z) + * @param int32_tegerHeightScale Scaling factor used to scale the height values (only when height values type is int32_teger) + */ HeightFieldShape(int32_t _nbGridColumns, int32_t _nbGridRows, float _minHeight, diff --git a/ephysics/collision/shapes/SphereShape.cpp b/ephysics/collision/shapes/SphereShape.cpp index f3ba7c2..7b5e036 100644 --- a/ephysics/collision/shapes/SphereShape.cpp +++ b/ephysics/collision/shapes/SphereShape.cpp @@ -10,17 +10,14 @@ #include #include +// TODO: REMOVE this ... using namespace ephysics; -// Constructor -/** - * @param radius Radius of the sphere (in meters) - */ -SphereShape::SphereShape(float radius) : ConvexShape(SPHERE, radius) { - assert(radius > 0.0f); +SphereShape::SphereShape(float _radius): + ConvexShape(SPHERE, _radius) { + assert(_radius > 0.0f); } - void SphereShape::setLocalScaling(const vec3& _scaling) { m_margin = (m_margin / m_scaling.x()) * _scaling.x(); CollisionShape::setLocalScaling(_scaling); @@ -52,49 +49,41 @@ void SphereShape::getLocalBounds(vec3& _min, vec3& _max) const { _min.setZ(_min.x()); } - -// Raycast method with feedback information -bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - - const vec3 m = ray.point1; +bool SphereShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const { + const vec3 m = _ray.point1; float c = m.dot(m) - m_margin * m_margin; - // If the origin of the ray is inside the sphere, we return no int32_tersection - if (c < 0.0f) return false; - - const vec3 rayDirection = ray.point2 - ray.point1; + if (c < 0.0f) { + return false; + } + const vec3 rayDirection = _ray.point2 - _ray.point1; float b = m.dot(rayDirection); - // If the origin of the ray is outside the sphere and the ray // is pointing away from the sphere, there is no int32_tersection - if (b > 0.0f) return false; - + if (b > 0.0f) { + return false; + } float raySquareLength = rayDirection.length2(); - // Compute the discriminant of the quadratic equation float discriminant = b * b - raySquareLength * c; - // If the discriminant is negative or the ray length is very small, there is no int32_tersection - if (discriminant < 0.0f || raySquareLength < FLT_EPSILON) return false; - + if ( discriminant < 0.0f + || raySquareLength < FLT_EPSILON) { + return false; + } // Compute the solution "t" closest to the origin float t = -b - etk::sqrt(discriminant); - assert(t >= 0.0f); - // If the hit point is withing the segment ray fraction - if (t < ray.maxFraction * raySquareLength) { - + if (t < _ray.maxFraction * raySquareLength) { // Compute the int32_tersection information t /= raySquareLength; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = ray.point1 + t * rayDirection; - raycastInfo.worldNormal = raycastInfo.worldPoint; - + _raycastInfo.body = _proxyShape->getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = _ray.point1 + t * rayDirection; + _raycastInfo.worldNormal = _raycastInfo.worldPoint; return true; } - return false; } diff --git a/ephysics/collision/shapes/SphereShape.hpp b/ephysics/collision/shapes/SphereShape.hpp index b29a615..ff6adb7 100644 --- a/ephysics/collision/shapes/SphereShape.hpp +++ b/ephysics/collision/shapes/SphereShape.hpp @@ -36,7 +36,10 @@ namespace ephysics { return sizeof(SphereShape); } public : - /// Constructor + /** + * @brief Constructor + * @param[in] radius Radius of the sphere (in meters) + */ SphereShape(float _radius); /** * @brief Get the radius of the sphere diff --git a/ephysics/collision/shapes/TriangleShape.cpp b/ephysics/collision/shapes/TriangleShape.cpp index da278ad..2a877fc 100644 --- a/ephysics/collision/shapes/TriangleShape.cpp +++ b/ephysics/collision/shapes/TriangleShape.cpp @@ -11,194 +11,153 @@ #include #include +// TODO: REMOVE this... using namespace ephysics; -// Constructor -/** - * @param point1 First point of the triangle - * @param point2 Second point of the triangle - * @param point3 Third point of the triangle - * @param margin The collision margin (in meters) around the collision shape - */ -TriangleShape::TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, float margin) - : ConvexShape(TRIANGLE, margin) { - m_points[0] = point1; - m_points[1] = point2; - m_points[2] = point3; +TriangleShape::TriangleShape(const vec3& _point1, const vec3& _point2, const vec3& _point3, float _margin) + : ConvexShape(TRIANGLE, _margin) { + m_points[0] = _point1; + m_points[1] = _point2; + m_points[2] = _point3; m_raycastTestType = FRONT; } -// Destructor -TriangleShape::~TriangleShape() { - -} - -// Raycast method with feedback information -/// This method use the line vs triangle raycasting technique described in -/// Real-time Collision Detection by Christer Ericson. -bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - +bool TriangleShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const { PROFILE("TriangleShape::raycast()"); - - const vec3 pq = ray.point2 - ray.point1; - const vec3 pa = m_points[0] - ray.point1; - const vec3 pb = m_points[1] - ray.point1; - const vec3 pc = m_points[2] - ray.point1; - + const vec3 pq = _ray.point2 - _ray.point1; + const vec3 pa = m_points[0] - _ray.point1; + const vec3 pb = m_points[1] - _ray.point1; + const vec3 pc = m_points[2] - _ray.point1; // Test if the line PQ is inside the eges BC, CA and AB. We use the triple // product for this test. const vec3 m = pq.cross(pc); float u = pb.dot(m); if (m_raycastTestType == FRONT) { - if (u < 0.0f) return false; + if (u < 0.0f) { + return false; + } + } else if (m_raycastTestType == BACK) { + if (u > 0.0f) { + return false; + } } - else if (m_raycastTestType == BACK) { - if (u > 0.0f) return false; - } - float v = -pa.dot(m); if (m_raycastTestType == FRONT) { - if (v < 0.0f) return false; + if (v < 0.0f) { + return false; + } + } else if (m_raycastTestType == BACK) { + if (v > 0.0f) { + return false; + } + } else if (m_raycastTestType == FRONT_AND_BACK) { + if (!sameSign(u, v)) { + return false; + } } - else if (m_raycastTestType == BACK) { - if (v > 0.0f) return false; - } - else if (m_raycastTestType == FRONT_AND_BACK) { - if (!sameSign(u, v)) return false; - } - float w = pa.dot(pq.cross(pb)); if (m_raycastTestType == FRONT) { - if (w < 0.0f) return false; + if (w < 0.0f) { + return false; + } + } else if (m_raycastTestType == BACK) { + if (w > 0.0f) { + return false; + } + } else if (m_raycastTestType == FRONT_AND_BACK) { + if (!sameSign(u, w)) { + return false; + } } - else if (m_raycastTestType == BACK) { - if (w > 0.0f) return false; - } - else if (m_raycastTestType == FRONT_AND_BACK) { - if (!sameSign(u, w)) return false; - } - // If the line PQ is in the triangle plane (case where u=v=w=0) - if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false; - + if ( approxEqual(u, 0) + && approxEqual(v, 0) + && approxEqual(w, 0)) { + return false; + } // Compute the barycentric coordinates (u, v, w) to determine the // int32_tersection point R, R = u * a + v * b + w * c float denom = 1.0f / (u + v + w); u *= denom; v *= denom; w *= denom; - // Compute the local hit point using the barycentric coordinates const vec3 localHitPoint = u * m_points[0] + v * m_points[1] + w * m_points[2]; - const float hitFraction = (localHitPoint - ray.point1).length() / pq.length(); - - if (hitFraction < 0.0f || hitFraction > ray.maxFraction) return false; - + const float hitFraction = (localHitPoint - _ray.point1).length() / pq.length(); + if ( hitFraction < 0.0f + || hitFraction > _ray.maxFraction) { + return false; + } vec3 localHitNormal = (m_points[1] - m_points[0]).cross(m_points[2] - m_points[0]); - if (localHitNormal.dot(pq) > 0.0f) localHitNormal = -localHitNormal; - - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.worldPoint = localHitPoint; - raycastInfo.hitFraction = hitFraction; - raycastInfo.worldNormal = localHitNormal; - + if (localHitNormal.dot(pq) > 0.0f) { + localHitNormal = -localHitNormal; + } + _raycastInfo.body = _proxyShape->getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.worldPoint = localHitPoint; + _raycastInfo.hitFraction = hitFraction; + _raycastInfo.worldNormal = localHitNormal; return true; } - -// Return the number of bytes used by the collision shape size_t TriangleShape::getSizeInBytes() const { return sizeof(TriangleShape); } -// Return a local support point in a given direction without the object margin -vec3 TriangleShape::getLocalSupportPointWithoutMargin(const vec3& direction, - void** cachedCollisionData) const { - vec3 dotProducts(direction.dot(m_points[0]), direction.dot(m_points[1]), direction.dot(m_points[2])); +vec3 TriangleShape::getLocalSupportPointWithoutMargin(const vec3& _direction, + void** _cachedCollisionData) const { + vec3 dotProducts(_direction.dot(m_points[0]), _direction.dot(m_points[1]), _direction.dot(m_points[2])); return m_points[dotProducts.getMaxAxis()]; } -// 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 TriangleShape::getLocalBounds(vec3& min, vec3& max) const { - +void TriangleShape::getLocalBounds(vec3& _min, vec3& _max) const { const vec3 xAxis(m_points[0].x(), m_points[1].x(), m_points[2].x()); const vec3 yAxis(m_points[0].y(), m_points[1].y(), m_points[2].y()); const vec3 zAxis(m_points[0].z(), m_points[1].z(), m_points[2].z()); - min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()); - max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()); - - min -= vec3(m_margin, m_margin, m_margin); - max += vec3(m_margin, m_margin, m_margin); + _min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()); + _max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()); + _min -= vec3(m_margin, m_margin, m_margin); + _max += vec3(m_margin, m_margin, m_margin); } -// Set the local scaling vector of the collision shape -void TriangleShape::setLocalScaling(const vec3& scaling) { - - m_points[0] = (m_points[0] / m_scaling) * scaling; - m_points[1] = (m_points[1] / m_scaling) * scaling; - m_points[2] = (m_points[2] / m_scaling) * scaling; - - CollisionShape::setLocalScaling(scaling); +void TriangleShape::setLocalScaling(const vec3& _scaling) { + m_points[0] = (m_points[0] / m_scaling) * _scaling; + m_points[1] = (m_points[1] / m_scaling) * _scaling; + m_points[2] = (m_points[2] / m_scaling) * _scaling; + CollisionShape::setLocalScaling(_scaling); } -// Return the local inertia tensor of the triangle shape -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - tensor.setZero(); +void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { + _tensor.setZero(); } -// Update the AABB of a body using its collision shape -/** - * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape - * computed in world-space coordinates - * @param transform etk::Transform3D used to compute the AABB of the collision shape - */ -void TriangleShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const { - - const vec3 worldPoint1 = transform * m_points[0]; - const vec3 worldPoint2 = transform * m_points[1]; - const vec3 worldPoint3 = transform * m_points[2]; - +void TriangleShape::computeAABB(AABB& _aabb, const etk::Transform3D& _transform) const { + const vec3 worldPoint1 = _transform * m_points[0]; + const vec3 worldPoint2 = _transform * m_points[1]; + const vec3 worldPoint3 = _transform * m_points[2]; const vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x()); const vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y()); const vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z()); - aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); - aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); + _aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); + _aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); } -// Return true if a point is inside the collision shape -bool TriangleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { +bool TriangleShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const { return false; } -// Return the raycast test type (front, back, front-back) TriangleRaycastSide TriangleShape::getRaycastTestType() const { return m_raycastTestType; } -// Set the raycast test type (front, back, front-back) -/** - * @param testType Raycast test type for the triangle (front, back, front-back) - */ -void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { - m_raycastTestType = testType; + +void TriangleShape::setRaycastTestType(TriangleRaycastSide _testType) { + m_raycastTestType = _testType; +} + +vec3 TriangleShape::getVertex(int32_t _index) const { + assert( _index >= 0 + && _index < 3); + return m_points[_index]; } -// Return the coordinates of a given vertex of the triangle -/** - * @param index Index (0 to 2) of a vertex of the triangle - */ -vec3 TriangleShape::getVertex(int32_t index) const { - assert(index >= 0 && index < 3); - return m_points[index]; -} \ No newline at end of file diff --git a/ephysics/collision/shapes/TriangleShape.hpp b/ephysics/collision/shapes/TriangleShape.hpp index 8043ff0..4f44350 100644 --- a/ephysics/collision/shapes/TriangleShape.hpp +++ b/ephysics/collision/shapes/TriangleShape.hpp @@ -25,34 +25,46 @@ namespace ephysics { * This class represents a triangle collision shape that is centered * at the origin and defined three points. */ - class TriangleShape : public ConvexShape { + class TriangleShape: public ConvexShape { protected: vec3 m_points[3]; //!< Three points of the triangle TriangleRaycastSide m_raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) /// Private copy-constructor - TriangleShape(const TriangleShape& shape); + TriangleShape(const TriangleShape& _shape); /// Private assignment operator - TriangleShape& operator=(const TriangleShape& shape); + TriangleShape& operator=(const TriangleShape& _shape); vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override; bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override; - bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override; size_t getSizeInBytes() const override; public: - /// Constructor - TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, - float margin = OBJECT_MARGIN); - /// Destructor - virtual ~TriangleShape(); - void getLocalBounds(vec3& min, vec3& max) const override; - void setLocalScaling(const vec3& scaling) override; - void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const override; - void computeAABB(AABB& aabb, const etk::Transform3D& transform) const override; + /** + * @brief Constructor + * @param _point1 First point of the triangle + * @param _point2 Second point of the triangle + * @param _point3 Third point of the triangle + * @param _margin The collision margin (in meters) around the collision shape + */ + TriangleShape(const vec3& _point1, + const vec3& _point2, + const vec3& _point3, + float _margin = OBJECT_MARGIN); + void getLocalBounds(vec3& _min, vec3& _max) const override; + void setLocalScaling(const vec3& _scaling) override; + void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override; + void computeAABB(AABB& aabb, const etk::Transform3D& _transform) const override; /// Return the raycast test type (front, back, front-back) TriangleRaycastSide getRaycastTestType() const; - // Set the raycast test type (front, back, front-back) - void setRaycastTestType(TriangleRaycastSide testType); - /// Return the coordinates of a given vertex of the triangle - vec3 getVertex(int32_t index) const; + /** + * @brief Set the raycast test type (front, back, front-back) + * @param[in] _testType Raycast test type for the triangle (front, back, front-back) + */ + void setRaycastTestType(TriangleRaycastSide _testType); + /** + * @brief Return the coordinates of a given vertex of the triangle + * @param[in] _index Index (0 to 2) of a vertex of the triangle + */ + vec3 getVertex(int32_t _index) const; friend class ConcaveMeshRaycastCallback; friend class TriangleOverlapCallback; };