Fix crash within testbed application in raycasting scene

DanielChappuis · May 16, 2024 · 6483ede · 6483ede
1 parent 4a1043b
commit 6483ede
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 25 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,6 +1,10 @@
 # Changelog
 
-## [Unreleased] - 2024-04-22
+## [Unreleased] - 2024-05-16
+
+### Fixed
+
+- Fix crash within testbed application in raycasting scene
 
 ## [0.10.0] - 2024-03-10
 

diff --git a/testbed/common/VisualContactPoint.cpp b/testbed/common/VisualContactPoint.cpp
@@ -28,32 +28,56 @@
 
 // Initialization of static variables
 openglframework::VertexBufferObject VisualContactPoint::mVBOVertices(GL_ARRAY_BUFFER);
+openglframework::VertexBufferObject VisualContactPoint::mVBOVerticesNormalLine(GL_ARRAY_BUFFER);
 openglframework::VertexBufferObject VisualContactPoint::mVBONormals(GL_ARRAY_BUFFER);
 openglframework::VertexBufferObject VisualContactPoint::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER);
 openglframework::VertexArrayObject VisualContactPoint::mVAO;
+openglframework::VertexArrayObject VisualContactPoint::mVAONormalLine;
+openglframework::Vector3 VisualContactPoint::mContactNormalLineStaticPoints[2] = {openglframework::Vector3(0, 0, 0), openglframework::Vector3(0, 1, 0)};
+
 int VisualContactPoint::mNbTotalPoints = 0;
 openglframework::Mesh VisualContactPoint::mMesh;
 bool VisualContactPoint::mStaticDataCreated = false;
 
 // Constructor
 VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position,
 									   const openglframework::Vector3& normalLineEndPointLocal, const openglframework::Color& color)
-                   : mVBOVerticesNormalLine(GL_ARRAY_BUFFER), mColor(color) {
+                   : mColor(color) {
 
 	mContactNormalLinePoints[0] = openglframework::Vector3(0, 0, 0);
 	mContactNormalLinePoints[1] = (normalLineEndPointLocal - position) * 0.5f;
 
     // Initialize the position where the mesh will be rendered
     translateWorld(position);
 
-	// Create the VBO and VAO to render the contact normal line
-	createContactNormalLineVBOAndVAO();
-}
-
-// Destructor
-VisualContactPoint::~VisualContactPoint() {
-	mVAONormalLine.destroy();
-	mVBOVerticesNormalLine.destroy();
+    // Compute the rotation quaternion from unit (0, 1, 0) to the actual normal vector in local-space
+    openglframework::Vector3 normalLineEndPointLocalVec(normalLineEndPointLocal.x, normalLineEndPointLocal.y, normalLineEndPointLocal.z);
+    openglframework::Vector3 positionVec(position.x, position.y, position.z);
+    openglframework::Vector3 v1(0, 1, 0);
+    openglframework::Vector3 v2 = (normalLineEndPointLocalVec - positionVec);
+    v2.normalize();
+    const float dot = v1.dot(v2);
+    openglframework::Vector3 xUnitVec(1, 0, 0);
+    openglframework::Vector3 yUnitVec(0, 1, 0);
+    openglframework::Vector3 rotVector;
+    // Handle parallel vectors
+    if (dot < -0.99999f) {
+        rotVector = xUnitVec.cross(v1);
+        if (rotVector.lengthSquared() < 0.000001f) {
+            rotVector = yUnitVec.cross(v1);
+        }
+        rotVector.normalize();
+        mNormalRotation.setAllValues(rotVector.x, rotVector.y, rotVector.z, openglframework::PI);
+    }
+    else if (dot > 0.99999f) {
+        mNormalRotation.setAllValues(0, 0, 0, 1);
+    }
+    else { // Handlee other vectors
+        rotVector = v1.cross(v2);
+        const float w = sqrt(v1.lengthSquared() * v2.lengthSquared());
+        mNormalRotation.setAllValues(rotVector.x, rotVector.y, rotVector.z, w + dot);
+        mNormalRotation.normalize();
+    }
 }
 
 // Load and initialize the mesh for all the contact points
@@ -69,7 +93,9 @@ void VisualContactPoint::createStaticData(const std::string& meshFolderPath) {
 
     mMesh.scaleVertices(VISUAL_CONTACT_POINT_RADIUS);
 
-    createVBOAndVAO();
+    createSphereVBOAndVAO();
+
+    createContactNormalLineVBOAndVAO();
 
     mStaticDataCreated = true;
 }
@@ -85,6 +111,9 @@ void VisualContactPoint::destroyStaticData() {
     mVBONormals.destroy();
     mVAO.destroy();
 
+    mVAONormalLine.destroy();
+    mVBOVerticesNormalLine.destroy();
+
     mMesh.destroy();
 
     mStaticDataCreated = false;
@@ -161,8 +190,13 @@ void VisualContactPoint::renderContactNormalLine(openglframework::Shader& shader
 	mVBOVerticesNormalLine.bind();
 
 	// Set the model to camera matrix
-	const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix;
-	shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix);
+    const rp3d::Matrix3x3 m = mNormalRotation.getMatrix();
+    const openglframework::Matrix4 contactNormalRotation(m[0][0], m[0][1], m[0][2], 0,
+                                                         m[1][0], m[1][1], m[1][2], 0,
+                                                         m[2][0], m[2][1], m[2][2], 0,
+                                                        0, 0, 0, 1);
+    const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix * contactNormalRotation;
+    shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix* contactNormalRotation);
 	shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
 
 	// Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
@@ -197,7 +231,7 @@ void VisualContactPoint::renderContactNormalLine(openglframework::Shader& shader
 
 // Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL.
 /// We create two VBOs (one for vertices and one for indices)
-void VisualContactPoint::createVBOAndVAO() {
+void VisualContactPoint::createSphereVBOAndVAO() {
 
     // Create the VBO for the vertices data
     mVBOVertices.create();
@@ -244,7 +278,7 @@ void VisualContactPoint::createContactNormalLineVBOAndVAO() {
 	mVBOVerticesNormalLine.create();
 	mVBOVerticesNormalLine.bind();
 	size_t sizeNormalLineVertices = 2 * sizeof(openglframework::Vector3);
-	mVBOVerticesNormalLine.copyDataIntoVBO(sizeNormalLineVertices, &mContactNormalLinePoints[0], GL_STATIC_DRAW);
+    mVBOVerticesNormalLine.copyDataIntoVBO(sizeNormalLineVertices, &mContactNormalLineStaticPoints[0], GL_STATIC_DRAW);
 	mVBOVerticesNormalLine.unbind();
 
 	// Create the VAO for both VBOs

diff --git a/testbed/common/VisualContactPoint.h b/testbed/common/VisualContactPoint.h
@@ -58,25 +58,31 @@ class VisualContactPoint : public openglframework::Object3D {
         static openglframework::VertexArrayObject mVAO;
 
 		/// Vertex Buffer Object for the vertices data
-		openglframework::VertexBufferObject mVBOVerticesNormalLine;
+        static openglframework::VertexBufferObject mVBOVerticesNormalLine;
 
 		/// Vertex Array Object for the vertex data
-		openglframework::VertexArrayObject mVAONormalLine;
+        static openglframework::VertexArrayObject mVAONormalLine;
 
         /// True if static data (VBO, VAO) has been created already
         static bool mStaticDataCreated;
 
-		// Two end-points of the contact normal line
+        /// Two end-points of the contact normal line (static points for the VBO)
+        static openglframework::Vector3 mContactNormalLineStaticPoints[2];
+
+        /// Two end-points of the contact normal line
 		openglframework::Vector3 mContactNormalLinePoints[2];
 
         /// Color
         openglframework::Color mColor;
 
-        // Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL.
-        static void createVBOAndVAO();
+        /// Rotation of the contact normal
+        rp3d::Quaternion mNormalRotation;
+
+        /// Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL.
+        static void createSphereVBOAndVAO();
 
-		// Create the Vertex Buffer Objects used to render the contact normal line with OpenGL.
-		void createContactNormalLineVBOAndVAO();
+        /// Create the Vertex Buffer Objects used to render the contact normal line with OpenGL.
+        static void createContactNormalLineVBOAndVAO();
 
 		/// Render the contact normal line
 		void renderContactNormalLine(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix);
@@ -91,9 +97,6 @@ class VisualContactPoint : public openglframework::Object3D {
         VisualContactPoint(const openglframework::Vector3& position, const openglframework::Vector3& normalLineEndPointLocal,
                            const openglframework::Color& color);
 
-        /// Destructor
-        ~VisualContactPoint();
-
         /// Load and initialize the mesh for all the contact points
         static void createStaticData(const std::string& meshFolderPath);