Merge pull request #230 from pariterre/stl_reader

Added an STL reader

CMakeLists.txt

@@ -3,7 +3,7 @@ if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.0")
     cmake_policy(SET CMP0042 NEW)
 endif()
 
-project(biorbd VERSION 1.7.2)
+project(biorbd VERSION 1.7.3)
 set(BIORBD_ROOT_FOLDER ${PROJECT_SOURCE_DIR})
 set (BIORBD_NAME_NO_SUFFIX ${PROJECT_NAME})
 set (CMAKE_CXX_STANDARD 11)

include/ModelReader.h

@@ -185,6 +185,14 @@ class BIORBD_API Reader
         const utils::Path& path);
 #endif
 
+    ///
+    /// \brief Read a STL file containing the meshing of a segment
+    /// \param path The path of the file
+    /// \return Returns the mesh
+    ///
+    static rigidbody::Mesh readMeshFileStl(
+        const utils::Path& path);
+
 protected:
     ///
     /// \brief Read a Vector 3d

include/Utils/IfStream.h

@@ -12,6 +12,8 @@ namespace BIORBD_NAMESPACE
 namespace utils
 {
 class Equation;
+class Vector3d;
+
 ///
 /// \brief Wrapper for the an std::ifstream with increased capacities
 ///
@@ -129,6 +131,31 @@ class BIORBD_API IfStream
         const std::map<Equation, double> &variables);
 #endif
 
+    ///
+    /// \brief Read a certain amounts of bits in binary
+    /// \param output Where the result is store
+    /// \param n_elements Number of bits to read
+    /// \return True on success
+    ///
+    bool readFromBinary(
+        char* output,
+        int n_elements);
+
+    ///
+    /// \brief Read a float in binary file
+    /// \param result The float to put the result into
+    /// \return True on success
+    ///
+    bool readFromBinary(
+        float& result);
+
+    ///
+    /// \brief Read a float in binary file
+    /// \return True on success
+    ///
+    bool readFromBinary(
+        Vector3d& result);
+
     ///
     /// \brief Advance in the file to a specific tag
     /// \param tag The tag to reach
@@ -142,10 +169,12 @@ class BIORBD_API IfStream
     ///
     /// \brief Advance in the file to a specific tag
     /// \param tag The tag to reach
+    /// \param maxTag The number of element to read before giving up
     /// \return True on success
     ///
     bool reachSpecificTag(
-        const String& tag);
+        const String& tag,
+        unsigned int maxTag = -1);
 
     ///
     /// \brief Counts the number of consecutive lines starting with the same tag and then brings it back to the initial position
@@ -162,6 +191,11 @@ class BIORBD_API IfStream
     void getline(
         String& text);
 
+    ///
+    /// \brief Reset the file cursor to the 0 position
+    ///
+    void resetCursor();
+
     ///
     /// \brief Close the file
     ///
@@ -175,6 +209,7 @@ class BIORBD_API IfStream
 
 protected:
     std::shared_ptr<bool> m_isOpen;///< If file is open
+    char m_floatBuffer[sizeof(float)]; ///< Buffer for reading float in binaries
 
 private:
     std::shared_ptr<std::ifstream> m_ifs;///< the ifstream

src/ModelReader.cpp

@@ -256,6 +256,9 @@ void Reader::readModelFile(
                             mesh = readMeshFileVtp(path.folder() + filePath.relativePath());
                         }
 #endif
+                        else if (!filePath.extension().tolower().compare("stl")) {
+                            mesh = readMeshFileStl(path.folder() + filePath.relativePath());
+                        }
                         else {
                             utils::Error::raise(filePath.extension() +
                                                         " is an unrecognized mesh file");
@@ -1984,6 +1987,98 @@ void Reader::readRtMatrix(
 }
 #endif  // MODULE_VTP_FILES_READER
 
+rigidbody::Mesh Reader::readMeshFileStl(
+    const utils::Path &path)
+{
+    // Read a bone file
+
+    // Open file
+    // std::cout << "Loading marker file: " << path << std::endl;
+#ifdef _WIN32
+    utils::IfStream file(
+        utils::Path::toWindowsFormat(
+            path.absolutePath()).c_str(), std::ios::in | std::ios::binary);
+#else
+    utils::IfStream file(
+        path.absolutePath().c_str(), std::ios::in | std::ios::binary);
+#endif
+
+    // The next step test if the file is ASCII or binary
+    bool isBinary = false;
+    try {
+        file.reachSpecificTag("facet", 20);
+    }  catch (std::runtime_error) {
+        isBinary = true;
+        file.resetCursor();
+    }
+
+    rigidbody::Mesh mesh;
+    if (isBinary){
+        // Know the number of points
+        char headerChar[80] = "";
+        char nbTrianglesChar[4];
+        char dummy[2];
+        file.readFromBinary(headerChar, 80); // Skip header
+        file.readFromBinary(nbTrianglesChar, 4);
+        unsigned int nbTriangles = *((unsigned int*) nbTrianglesChar);
+
+        mesh.setPath(path);
+        utils::Vector3d normal;
+        utils::Vector3d vertex;
+        for (unsigned int i = 0; i<nbTriangles; ++i){
+            file.readFromBinary(normal);
+            for (unsigned int j = 0; j<3; ++j){
+                file.readFromBinary(vertex);
+                mesh.addPoint(vertex);
+            }
+            file.readFromBinary(dummy, 2);
+
+            rigidbody::MeshFace patchTp;
+            patchTp(0) = 3*i + 0;
+            patchTp(1) = 3*i + 1;
+            patchTp(2) = 3*i + 2;
+            mesh.addFace(patchTp);
+        }
+    } else {
+        // Due to the test, the pointer is already at the first "facet"
+
+        // Read file
+        utils::String tp;
+        unsigned int i = 0;
+        utils::Vector3d vertex;
+        while (true){
+            file.reachSpecificTag("vertex");
+            for (unsigned int k=0; k<3; ++k){
+                for (unsigned int j=0; j<3; ++j) {
+                    file.read(vertex(j));
+                }
+                mesh.addPoint(vertex);
+                file.read(tp);
+            }
+
+            rigidbody::MeshFace patchTp;
+            patchTp(0) = 3*i + 0;
+            patchTp(1) = 3*i + 1;
+            patchTp(2) = 3*i + 2;
+            mesh.addFace(patchTp);
+
+            for (unsigned int j=0; j<3; ++j) {
+                // Read 3 dummies
+                file.read(tp);
+            }
+
+            ++i;
+            if (!tp.compare("endsolid")){
+                break;
+            }
+        }
+
+
+    }
+
+    return mesh;
+}
+
 
 std::vector<std::vector<utils::Vector3d>>
         Reader::readViconMarkerFile(const utils::Path &path,

src/Utils/IfStream.cpp

@@ -5,6 +5,7 @@
 #include <fstream>
 #include "Utils/Error.h"
 #include "Utils/Equation.h"
+#include "Utils/Vector3d.h"
 
 using namespace BIORBD_NAMESPACE;
 
@@ -59,13 +60,19 @@ bool utils::IfStream::readSpecificTag(
     read(text);
     return true;
 }
-bool utils::IfStream::reachSpecificTag(const utils::String& tag)
+bool utils::IfStream::reachSpecificTag(
+        const utils::String& tag,
+        unsigned int maxTag)
 {
     utils::String text;
-    while (read(text))
+    unsigned int i = 0;
+    while (read(text) && i < maxTag){
         if (!text.tolower().compare(tag)) {
             return true;
         }
+        ++i;
+    }
+
 
     utils::String outMessage(tag +
                                      " parameter could not be found in Data file..");
@@ -156,6 +163,38 @@ bool utils::IfStream::read(
     }
     return out;
 }
+
+bool utils::IfStream::readFromBinary(
+        char *output,
+        int n_elements)
+{
+    m_ifs->read(output, n_elements);
+    return true;
+}
+
+bool utils::IfStream::readFromBinary(
+        float& output)
+{
+    m_ifs->read(m_floatBuffer, 4);
+    output = *((float*) m_floatBuffer);
+    return true;
+}
+
+bool utils::IfStream::readFromBinary(
+        utils::Vector3d& v)
+{
+    m_ifs->read(m_floatBuffer, 4);
+    v[0] = *((float*) m_floatBuffer);
+
+    m_ifs->read(m_floatBuffer, 4);
+    v[1] = *((float*) m_floatBuffer);
+
+    m_ifs->read(m_floatBuffer, 4);
+    v[2] = *((float*) m_floatBuffer);
+    return true;
+}
+
+
 #ifdef BIORBD_USE_CASADI_MATH
 bool utils::IfStream::read(
     RBDLCasadiMath::MX_Xd_SubMatrix result,
@@ -203,6 +242,12 @@ void utils::IfStream::getline(utils::String& text)
     std::getline(*m_ifs, text);
 }
 
+void utils::IfStream::resetCursor()
+{
+    m_ifs->clear();
+    m_ifs->seekg(0);
+}
+
 
 // Close the file
 bool utils::IfStream::close()