Add boundary checks for trimesh creation

SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp

@@ -228,11 +228,14 @@ namespace SimDynamics
             sc = 0.001f * ScaleFactor;
         }
 
-        for (size_t i = 0; i < trimesh->faces.size(); i++)
+        for (auto & face : trimesh->faces)
         {
-            btVector3 v1(btScalar((trimesh->vertices[trimesh->faces[i].id1][0] - com[0])*sc), btScalar((trimesh->vertices[trimesh->faces[i].id1][1] - com[1])*sc), btScalar((trimesh->vertices[trimesh->faces[i].id1][2] - com[2])*sc));
-            btVector3 v2(btScalar((trimesh->vertices[trimesh->faces[i].id2][0] - com[0])*sc), btScalar((trimesh->vertices[trimesh->faces[i].id2][1] - com[1])*sc), btScalar((trimesh->vertices[trimesh->faces[i].id2][2] - com[2])*sc));
-            btVector3 v3(btScalar((trimesh->vertices[trimesh->faces[i].id3][0] - com[0])*sc), btScalar((trimesh->vertices[trimesh->faces[i].id3][1] - com[1])*sc), btScalar((trimesh->vertices[trimesh->faces[i].id3][2] - com[2])*sc));
+            auto & vertex1 = trimesh->vertices.at(face.id1);
+            auto & vertex2 = trimesh->vertices.at(face.id2);
+            auto & vertex3 = trimesh->vertices.at(face.id3);
+            btVector3 v1(btScalar((vertex1[0] - com[0])*sc), btScalar((vertex1[1] - com[1])*sc), btScalar((vertex1[2] - com[2])*sc));
+            btVector3 v2(btScalar((vertex2[0] - com[0])*sc), btScalar((vertex2[1] - com[1])*sc), btScalar((vertex2[2] - com[2])*sc));
+            btVector3 v3(btScalar((vertex3[0] - com[0])*sc), btScalar((vertex3[1] - com[1])*sc), btScalar((vertex3[2] - com[2])*sc));
             btTrimesh->addTriangle(v1, v2, v3);
         }