Fixed index bug in the new external force

pyomeca · Nov 13, 2023 · a40ca3d · a40ca3d
1 parent c7c6da4
commit a40ca3d
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 5 deletions.
diff --git a/include/ModelReader.h b/include/ModelReader.h
@@ -1,6 +1,7 @@
 #ifndef BIORBD_UTILS_READ_H
 #define BIORBD_UTILS_READ_H
 
+#include <stddef.h>
 #include <vector>
 #include <map>
 #include "biorbdConfig.h"

diff --git a/src/ModelReader.cpp b/src/ModelReader.cpp
@@ -2282,7 +2282,7 @@ rigidbody::Mesh Reader::readMeshFileStl(
         char dummy[2];
         file.readFromBinary(headerChar, 80); // Skip header
         file.readFromBinary(nbTrianglesChar, 4);
-        size_t nbTriangles = *((size_t*) nbTrianglesChar);
+        size_t nbTriangles = static_cast<size_t>(*((unsigned int*) nbTrianglesChar));
 
         mesh.setPath(path);
         utils::Vector3d normal;

diff --git a/src/RigidBody/ExternalForceSet.cpp b/src/RigidBody/ExternalForceSet.cpp
@@ -44,8 +44,9 @@ void rigidbody::ExternalForceSet::add(
     const utils::SpatialVector& vector
 ) 
 {
-    size_t dofIndex = m_model.segment(segmentName).getLastDofIndexInGeneralizedCoordinates(m_model);
-    m_externalForces[dofIndex] += vector; // Do not subtract 1 since 0 is used for the base ground 
+    // Add 1 since 0 is used for the base ground 
+    size_t dofIndex = m_model.segment(segmentName).getLastDofIndexInGeneralizedCoordinates(m_model) + 1;
+    m_externalForces[dofIndex] += vector; 
 }
 
 void rigidbody::ExternalForceSet::add(
@@ -273,7 +274,7 @@ void rigidbody::ExternalForceSet::combineTranslationalForces(
 
     for (size_t i = 0; i <m_model.nbSegment(); ++i) {
         const rigidbody::Segment& segment(m_model.segment(i));
-        //  (Add 1 to account for the undeclared root
+        // Add 1 to account for the undeclared root
         size_t dofIndex = segment.getLastDofIndexInGeneralizedCoordinates(m_model) + 1;
 
         for (auto& e : m_translationalForces) {    
@@ -319,7 +320,7 @@ void rigidbody::ExternalForceSet::combineSoftContactForces(
 
     for (size_t i = 0; i < m_model.nbSegment(); ++i) {
         const rigidbody::Segment& segment(m_model.segment(i));
-        //  Add 1 to account for the undeclared root
+        // Add 1 to account for the undeclared root
         size_t dofIndex = segment.getLastDofIndexInGeneralizedCoordinates(m_model) + 1;
 
         for (size_t j = 0; j < m_model.nbSoftContacts(); j++) {