From a40ca3df6b6ae4839593f8bc2d5e975fd036452c Mon Sep 17 00:00:00 2001
From: Pariterre <pariterre@hotmail.com>
Date: Mon, 13 Nov 2023 16:06:55 -0500
Subject: [PATCH] Fixed index bug in the new external force

---
 include/ModelReader.h              | 1 +
 src/ModelReader.cpp                | 2 +-
 src/RigidBody/ExternalForceSet.cpp | 9 +++++----
 3 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/include/ModelReader.h b/include/ModelReader.h
index 0ea01687..03ce20b9 100644
--- 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
index e03e6efe..9b12c719 100644
--- 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
index b0fb72c6..4de3863d 100644
--- 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++) {