Fix issue with scaling of ConcaveMeshShape and HeightFieldShape norma…

…ls, display collider normals in DebugRenderer

CHANGELOG.md

@@ -7,7 +7,9 @@
  - The library will now return errors found in input data during the creation of ConvexMesh, TriangularMesh and HeighField
  - It is now possible to create a ConvexMesh by specifying only a list of vertices (automatic computation of convex hull using internal QuickHull algorithm)
  - The performance of static bodies has been improved 
- - The reporting of contact state is now correct even if body goes to sleep
+ - The reporting of contact state is now correct even if the body goes to sleep
+ - The DebugRenderer can now display the normals of the collider faces for debugging purpose
+ - It is now possible to select for which bodies the debug information from the DebugRenderer is displayed
 
 ### Changed
 
@@ -54,7 +56,11 @@
 
 ### Fixed
 
+- Issue [#206](https://github.com/DanielChappuis/reactphysics3d/issues/206) Collision issue and scaling of collider normals
+- Issue [#235](https://github.com/DanielChappuis/reactphysics3d/issues/235) Removing a body should wake up its neighbors
 - Issue [#237](https://github.com/DanielChappuis/reactphysics3d/issues/237) Wrong assert has been removed 
+- Issue [#239](https://github.com/DanielChappuis/reactphysics3d/issues/239) Memory allocation alignment
+- Issue [#240](https://github.com/DanielChappuis/reactphysics3d/issues/240) Uninitialized variable
 
 ## Version 0.9.0 (January 4, 2022)
 

include/reactphysics3d/collision/TriangleMesh.h

@@ -179,11 +179,6 @@ RP3D_FORCE_INLINE void TriangleMesh::getTriangleVerticesNormals(uint32 triangleI
     outN1 = mVerticesNormals[mTriangles[triangleIndex * 3]];
     outN2 = mVerticesNormals[mTriangles[triangleIndex * 3 + 1]];
     outN3 = mVerticesNormals[mTriangles[triangleIndex * 3 + 2]];
-
-    // TODO : REMOVE
-    assert(outN1.length() > MACHINE_EPSILON);
-    assert(outN2.length() > MACHINE_EPSILON);
-    assert(outN3.length() > MACHINE_EPSILON);
 }
 
 // Return the coordinates of a given vertex

include/reactphysics3d/collision/shapes/BoxShape.h

@@ -89,7 +89,7 @@ class BoxShape : public ConvexPolyhedronShape {
         BoxShape& operator=(const BoxShape& shape) = delete;
 
         /// Return the half-extents of the box
-        Vector3 getHalfExtents() const;
+        const Vector3& getHalfExtents() const;
 
         /// Set the half-extents of the box
         void setHalfExtents(const Vector3& halfExtents);
@@ -142,7 +142,7 @@ class BoxShape : public ConvexPolyhedronShape {
 /**
  * @return The vector with the three half-extents of the box shape
  */
-RP3D_FORCE_INLINE Vector3 BoxShape::getHalfExtents() const {
+RP3D_FORCE_INLINE const Vector3& BoxShape::getHalfExtents() const {
     return mHalfExtents;
 }
 

include/reactphysics3d/collision/shapes/ConcaveMeshShape.h

@@ -133,6 +133,9 @@ class ConcaveMeshShape : public ConcaveShape {
         /// Reference to the triangle half-edge structure
         HalfEdgeStructure& mTriangleHalfEdgeStructure;
 
+        /// Array with the scaled face normals
+        Array<Vector3> mScaledVerticesNormals;
+
         // -------------------- Methods -------------------- //
 
         /// Constructor
@@ -161,6 +164,9 @@ class ConcaveMeshShape : public ConcaveShape {
         // Return the integer data of leaf node of the dynamic AABB tree
         int32 getDynamicAABBTreeNodeDataInt(int32 nodeID) const;
 
+        /// Compute the scaled faces normals
+        void computeScaledVerticesNormals();
+
     public:
 
         /// Deleted copy-constructor
@@ -169,6 +175,9 @@ class ConcaveMeshShape : public ConcaveShape {
         /// Deleted assignment operator
         ConcaveMeshShape& operator=(const ConcaveMeshShape& shape) = delete;
 
+        /// Set the scale of the shape
+        virtual void setScale(const Vector3& scale) override;
+
         /// Return the number of vertices in the mesh
         uint32 getNbVertices() const;
 
@@ -187,7 +196,7 @@ class ConcaveMeshShape : public ConcaveShape {
                                         Vector3& outN2, Vector3& outN3) const;
 
         /// Return the coordinates of a given vertex
-        const Vector3& getVertex(uint32 vertexIndex) const;
+        const Vector3 getVertex(uint32 vertexIndex) const;
 
         /// Return the normal of a given vertex
         const Vector3& getVertexNormal(uint32 vertexIndex) const;

include/reactphysics3d/collision/shapes/ConcaveShape.h

@@ -99,7 +99,7 @@ class ConcaveShape : public CollisionShape {
         const Vector3& getScale() const;
 
         /// Set the scale of the shape
-        void setScale(const Vector3& scale);
+        virtual void setScale(const Vector3& scale);
 
         /// Return the local inertia tensor of the collision shape
         virtual Vector3 getLocalInertiaTensor(decimal mass) const override;

include/reactphysics3d/collision/shapes/ConvexMeshShape.h

@@ -57,6 +57,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
         /// Scale to apply to the mesh
         Vector3 mScale;
 
+        /// Array with the scaled face normals
+        Array<Vector3> mScaledFacesNormals;
+
         // -------------------- Methods -------------------- //
 
         /// Constructor
@@ -74,6 +77,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;
 
+        // Compute the scaled faces normals
+        void computeScaledFacesNormals();
+
         /// Destructor
         virtual ~ConvexMeshShape() override = default;
 
@@ -151,7 +157,12 @@ RP3D_FORCE_INLINE const Vector3& ConvexMeshShape::getScale() const {
 /// Note that you might want to recompute the inertia tensor and center of mass of the body
 /// after changing the scale of a collision shape
 RP3D_FORCE_INLINE void ConvexMeshShape::setScale(const Vector3& scale) {
+
     mScale = scale;
+
+    // Recompute the scaled face normals
+    computeScaledFacesNormals();
+
     notifyColliderAboutChangedSize();
 }
 
@@ -207,7 +218,7 @@ RP3D_FORCE_INLINE Vector3 ConvexMeshShape::getVertexPosition(uint32 vertexIndex)
 // Return the normal vector of a given face of the mesh
 RP3D_FORCE_INLINE Vector3 ConvexMeshShape::getFaceNormal(uint32 faceIndex) const {
     assert(faceIndex < getNbFaces());
-    return mConvexMesh->getFaceNormal(faceIndex);
+    return mScaledFacesNormals[faceIndex];
 }
 
 // Return the centroid of the mesh

include/reactphysics3d/utils/DebugRenderer.h

@@ -40,6 +40,7 @@ namespace reactphysics3d {
 class ConcaveMeshShape;
 class ConvexMeshShape;
 class HeightFieldShape;
+class BoxShape;
 class Collider;
 class PhysicsWorld;
 
@@ -86,6 +87,9 @@ class DebugRenderer : public EventListener {
 
             /// Display the contact normals
             CONTACT_NORMAL				= 1 << 4,
+
+            /// Display the face normals of the collision shapes
+            COLLISION_SHAPE_NORMAL		= 1 << 5,
         };
 
 		/// Struture that represents a line of the DebugRenderer
@@ -151,10 +155,13 @@ class DebugRenderer : public EventListener {
         /// Default radius of the sphere displayed to represent contact points
         static constexpr decimal DEFAULT_CONTACT_POINT_SPHERE_RADIUS = decimal(0.1);
 
-        /// Default radius of the sphere displayed to represent contact points
+        /// Default length for the displayed contacts normals
         static constexpr decimal DEFAULT_CONTACT_NORMAL_LENGTH = decimal(1.0);
 
-		// -------------------- Attributes -------------------- //
+        /// Default length for the displayed faces normals of the collision shapes
+        static constexpr decimal DEFAULT_COLLISION_SHAPE_NORMAL_LENGTH = decimal(1.0);
+
+        // -------------------- Attributes -------------------- //
 
 		/// Memory allocator
 		MemoryAllocator& mAllocator;
@@ -174,16 +181,19 @@ class DebugRenderer : public EventListener {
         /// Radius of the sphere displayed to represent contact points
         decimal mContactPointSphereRadius;
 
-        /// Lenght of contact normal
+        /// Length of contact normal
         decimal mContactNormalLength;
 
+        /// Length of collision shape face normal
+        decimal mCollisionShapeNormalLength;
+
         // -------------------- Methods -------------------- //
 
 		/// Draw an AABB
 		void drawAABB(const AABB& aabb, uint32 color);
 
 		/// Draw a box
-		void drawBox(const Transform& transform, const Vector3& extents, uint32 color);
+        void drawBox(const Transform& transform, const BoxShape* boxShape, uint32 colorShape, uint32 colorShapeNormals);
 
 		/// Draw a sphere
 		void drawSphere(const Vector3& position, decimal radius, uint32 color);
@@ -192,13 +202,15 @@ class DebugRenderer : public EventListener {
 		void drawCapsule(const Transform& transform, decimal radius, decimal height, uint32 color);
 
 		/// Draw a convex mesh
-		void drawConvexMesh(const Transform& transform, const ConvexMeshShape* convexMesh, uint32 color);
+        void drawConvexMesh(const Transform& transform, const ConvexMeshShape* convexMesh, uint32 colorShape, uint32 colorShapeNormals);
 
 		/// Draw a concave mesh shape
-		void drawConcaveMeshShape(const Transform& transform, const ConcaveMeshShape* concaveMeshShape, uint32 color);
+        void drawConcaveMeshShape(const Transform& transform, const ConcaveMeshShape* concaveMeshShape,
+                                  uint32 colorShape, uint32 colorShapeNormals);
 
 		/// Draw a height field shape
-		void drawHeightFieldShape(const Transform& transform, const HeightFieldShape* heightFieldShape, uint32 color);
+        void drawHeightFieldShape(const Transform& transform, const HeightFieldShape* heightFieldShape,
+                                  uint32 colorShape, uint32 colorShapeNormals);
 
 		/// Draw the collision shape of a collider
 		void drawCollisionShapeOfCollider(const Collider* collider, uint32 color);

src/collision/shapes/ConcaveMeshShape.cpp

@@ -34,10 +34,45 @@ using namespace reactphysics3d;
 
 // Constructor
 ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh, MemoryAllocator& allocator, HalfEdgeStructure& triangleHalfEdgeStructure, const Vector3& scaling)
-                 : ConcaveShape(CollisionShapeName::TRIANGLE_MESH, allocator, scaling), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure) {
+                 : ConcaveShape(CollisionShapeName::TRIANGLE_MESH, allocator, scaling), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure),
+                   mScaledVerticesNormals(allocator, triangleMesh->getNbVertices()) {
 
     mTriangleMesh = triangleMesh;
     mRaycastTestType = TriangleRaycastSide::FRONT;
+
+    computeScaledVerticesNormals();
+}
+
+// Set the scale of the shape
+void ConcaveMeshShape::setScale(const Vector3& scale) {
+
+    ConcaveShape::setScale(scale);
+
+    // Recompute the scale vertices normals
+    computeScaledVerticesNormals();
+}
+
+// Compute the scaled faces normals
+void ConcaveMeshShape::computeScaledVerticesNormals() {
+
+    mScaledVerticesNormals.clear();
+
+    // For each vertex
+    const uint32 nbVertices = mTriangleMesh->getNbVertices();
+    for (uint32 v=0; v < nbVertices; v++) {
+
+        Vector3 normal = mTriangleMesh->getVertexNormal(v);
+
+        // Scale the normal
+        normal = Vector3(1.0 / mScale.x, 1.0 / mScale.y, 1.0 / mScale.z) * normal;
+
+        // Normalize the normal
+        const decimal normalLength = normal.length();
+        assert(normalLength > MACHINE_EPSILON);
+        normal /= normalLength;
+
+        mScaledVerticesNormals.add(normal);
+    }
 }
 
 // Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
@@ -53,11 +88,18 @@ void ConcaveMeshShape::getTriangleVertices(uint32 triangleIndex, Vector3& outV1,
 }
 
 // Return the three vertex normals (in the array outVerticesNormals) of a triangle
-void ConcaveMeshShape::getTriangleVerticesNormals(uint32 triangleIndex,
-                                                  Vector3& outN1, Vector3& outN2, Vector3& outN3) const {
+void ConcaveMeshShape::getTriangleVerticesNormals(uint32 triangleIndex, Vector3& outN1, Vector3& outN2, Vector3& outN3) const {
 
-    // Get the vertices coordinates of the triangle
-    mTriangleMesh->getTriangleVerticesNormals(triangleIndex, outN1, outN2, outN3);
+    assert(triangleIndex < mTriangleMesh->getNbTriangles());
+
+    // Get the triangle vertices indices
+    uint32 v1, v2, v3;
+    mTriangleMesh->getTriangleVerticesIndices(triangleIndex, v1, v2, v3);
+
+    // Return the scaled vertices normals
+    outN1 = mScaledVerticesNormals[v1];
+    outN2 = mScaledVerticesNormals[v2];
+    outN3 = mScaledVerticesNormals[v3];
 }
 
 // Return the indices of the three vertices of a given triangle in the array
@@ -73,11 +115,22 @@ uint32 ConcaveMeshShape::getNbVertices() const {
 }
 
 // Return the number of triangles in a sub part of the mesh
-uint32 ConcaveMeshShape::getNbTriangles() const
-{
+uint32 ConcaveMeshShape::getNbTriangles() const {
     return mTriangleMesh->getNbTriangles();
 }
 
+// Return the coordinates of a given vertex
+const Vector3 ConcaveMeshShape::getVertex(uint32 vertexIndex) const {
+    assert(vertexIndex < mTriangleMesh->getNbVertices());
+    return mTriangleMesh->getVertex(vertexIndex) * mScale;
+}
+
+// Return the normal of a given vertex
+const Vector3& ConcaveMeshShape::getVertexNormal(uint32 vertexIndex) const {
+    assert(vertexIndex < mTriangleMesh->getNbVertices());
+    return mScaledVerticesNormals[vertexIndex];
+}
+
 // Compute all the triangles of the mesh that are overlapping with the AABB in parameter
 void ConcaveMeshShape::computeOverlappingTriangles(const AABB& localAABB, Array<Vector3>& triangleVertices,
                                                    Array<Vector3>& triangleVerticesNormals, Array<uint32>& shapeIds,
@@ -263,7 +316,7 @@ std::string ConcaveMeshShape::to_string() const {
     // For each vertex of the concave mesh
     for (uint32 v=0; v<getNbVertices(); v++) {
 
-        Vector3 normal = mTriangleMesh->getVertexNormal(v);
+        Vector3 normal = mScaledVerticesNormals[v];
 
         ss << normal.to_string() << ", ";
     }

src/collision/shapes/ConvexMeshShape.cpp

@@ -41,8 +41,32 @@ using namespace reactphysics3d;
  */
 ConvexMeshShape::ConvexMeshShape(ConvexMesh* convexMesh, MemoryAllocator& allocator, const Vector3& scale)
                 : ConvexPolyhedronShape(CollisionShapeName::CONVEX_MESH, allocator), mConvexMesh(convexMesh),
-                  mScale(scale) {
+                  mScale(scale), mScaledFacesNormals(allocator, convexMesh->getNbFaces()) {
 
+    computeScaledFacesNormals();
+}
+
+// Compute the scaled faces normals
+void ConvexMeshShape::computeScaledFacesNormals() {
+
+    mScaledFacesNormals.clear();
+
+    // For each face
+    const uint32 nbFaces = mConvexMesh->getNbFaces();
+    for (uint32 f=0; f < nbFaces; f++) {
+
+        Vector3 normal = mConvexMesh->getFaceNormal(f);
+
+        // Scale the normal
+        normal = Vector3(1.0 / mScale.x, 1.0 / mScale.y, 1.0 / mScale.z) * normal;
+
+        // Normalize the normal
+        const decimal normalLength = normal.length();
+        assert(normalLength > MACHINE_EPSILON);
+        normal /= normalLength;
+
+        mScaledFacesNormals.add(normal);
+    }
 }
 
 // Return a local support point in a given direction without the object margin.
@@ -96,7 +120,7 @@ bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider
     for (uint32 f=0; f < mConvexMesh->getNbFaces(); f++) {
 
         const HalfEdgeStructure::Face& face = halfEdgeStructure.getFace(f);
-        const Vector3& faceNormal = mConvexMesh->getFaceNormal(f);
+        const Vector3& faceNormal = getFaceNormal(f);
         const HalfEdgeStructure::Vertex& faceVertex = halfEdgeStructure.getVertex(face.faceVertices[0]);
         const Vector3& facePoint = mConvexMesh->getVertex(faceVertex.vertexPointIndex);
         decimal denom = faceNormal.dot(direction);
@@ -166,7 +190,7 @@ bool ConvexMeshShape::testPointInside(const Vector3& localPoint, Collider* /*col
     for (uint32 f=0; f < mConvexMesh->getNbFaces(); f++) {
 
         const HalfEdgeStructure::Face& face = halfEdgeStructure.getFace(f);
-        const Vector3& faceNormal = mConvexMesh->getFaceNormal(f);
+        const Vector3& faceNormal = getFaceNormal(f);
         const HalfEdgeStructure::Vertex& faceVertex = halfEdgeStructure.getVertex(face.faceVertices[0]);
         const Vector3& facePoint = mConvexMesh->getVertex(faceVertex.vertexPointIndex);