feat: visualize opensim muscle paths and wraps

src/OpenSimLoader.cpp

@@ -1,22 +1,25 @@
 #include "OpenSimLoader.hpp"
 
 #include <OpenSim/Common/Storage.h>
-#include <OpenSim/Simulation/Model/Appearance.h>
 #include <OpenSim/Simulation/Model/Geometry.h>
+#include <OpenSim/Simulation/Model/GeometryPath.h>
 #include <OpenSim/Simulation/Model/Marker.h>
 #include <OpenSim/Simulation/Model/Model.h>
 #include <OpenSim/Simulation/Model/ModelVisualizer.h>
 #include <OpenSim/Simulation/StatesTrajectory.h>
+#include <OpenSim/Simulation/Wrap/PathWrapPoint.h>
+#include <OpenSim/Simulation/Wrap/WrapCylinder.h>
+#include <OpenSim/Simulation/Wrap/WrapEllipsoid.h>
+#include <OpenSim/Simulation/Wrap/WrapSphere.h>
+#include <OpenSim/Simulation/Wrap/WrapTorus.h>
 #include <SimTKcommon/internal/PolygonalMesh.h>
 
 #include <Magnum/Math/Functions.h>
 #include <Magnum/Math/Matrix4.h>
 
-#include <algorithm>
 #include <cmath>
 #include <filesystem>
 #include <iostream>
-#include <limits>
 #include <memory>
 #include <set>
 #include <stdexcept>
@@ -24,11 +27,14 @@
 #include <unordered_map>
 
 using namespace Magnum;
+using namespace Math::Literals;
 
 namespace osim_viewer {
 
 namespace {
 
+constexpr Float TwoPi = 6.28318530717958647692f;
+
 Vector3 toVector3(const SimTK::Vec3& value) {
     return {Float(value[0]), Float(value[1]), Float(value[2])};
 }
@@ -38,30 +44,26 @@ Quaternion toQuaternion(const SimTK::Rotation& rotation) {
     return Quaternion{{Float(quat[1]), Float(quat[2]), Float(quat[3])}, Float(quat[0])}.normalized();
 }
 
-Color4 toColor(const OpenSim::Appearance& appearance) {
+Matrix4 toMatrix4(const SimTK::Transform& transform) {
-    const SimTK::Vec3 color = appearance.get_color();
+    return Matrix4::from(toQuaternion(transform.R()).toMatrix(), toVector3(transform.T()));
-    return {Float(color[0]), Float(color[1]), Float(color[2]), Float(appearance.get_opacity())};
 }
 
-Range3D scaledBounds(const Range3D& bounds, const Vector3& scale) {
+Color4 toColor(const SimTK::Vec3& color, const Float opacity = 1.0f) {
-    bool hasBounds = false;
+    return {Float(color[0]), Float(color[1]), Float(color[2]), opacity};
-    Range3D out;
-    for(const Vector3& corner: {
-            Vector3{bounds.min().x(), bounds.min().y(), bounds.min().z()},
-            Vector3{bounds.min().x(), bounds.min().y(), bounds.max().z()},
-            Vector3{bounds.min().x(), bounds.max().y(), bounds.min().z()},
-            Vector3{bounds.min().x(), bounds.max().y(), bounds.max().z()},
-            Vector3{bounds.max().x(), bounds.min().y(), bounds.min().z()},
-            Vector3{bounds.max().x(), bounds.min().y(), bounds.max().z()},
-            Vector3{bounds.max().x(), bounds.max().y(), bounds.min().z()},
-            Vector3{bounds.max().x(), bounds.max().y(), bounds.max().z()},
-        }) {
-        const Range3D point = Range3D::fromSize(corner*scale, {});
-        if(hasBounds) out = Math::join(out, point);
-        else out = point;
-        hasBounds = true;
 }
-    return out;
+
+Color4 toColor(const OpenSim::Appearance& appearance) {
+    return toColor(appearance.get_color(), Float(appearance.get_opacity()));
+}
+
+Color4 pathColor(const OpenSim::GeometryPath& path, const SimTK::State& state) {
+    const SimTK::Vec3 runtimeColor = path.getColor(state);
+    if(std::isfinite(runtimeColor[0]) &&
+       std::isfinite(runtimeColor[1]) &&
+       std::isfinite(runtimeColor[2])) {
+        return toColor(runtimeColor, Float(path.get_Appearance().get_opacity()));
+    }
+    return toColor(path.get_Appearance());
 }
 
 Range3D transformBounds(const Range3D& bounds, const Matrix4& transform) {
@@ -106,15 +108,13 @@ std::shared_ptr<CpuMesh> buildCpuMesh(const SimTK::PolygonalMesh& mesh) {
         for(int i = 1; i < faceVertexCount - 1; ++i) {
             const int b = mesh.getFaceVertex(face, i);
             const int c = mesh.getFaceVertex(face, i + 1);
-            const Vector3 ab = positions[b] - positions[a];
+            const Vector3 faceNormal = Math::cross(positions[b] - positions[a], positions[c] - positions[a]);
-            const Vector3 ac = positions[c] - positions[a];
-            const Vector3 faceNormal = Math::cross(ab, ac);
             normals[a] += faceNormal;
             normals[b] += faceNormal;
             normals[c] += faceNormal;
-            cpu->indices.push_back(Magnum::UnsignedInt(a));
+            cpu->indices.push_back(UnsignedInt(a));
-            cpu->indices.push_back(Magnum::UnsignedInt(b));
+            cpu->indices.push_back(UnsignedInt(b));
-            cpu->indices.push_back(Magnum::UnsignedInt(c));
+            cpu->indices.push_back(UnsignedInt(c));
         }
     }
 
@@ -135,9 +135,7 @@ std::shared_ptr<CpuMesh> buildCpuMesh(const SimTK::PolygonalMesh& mesh) {
 }
 
 std::shared_ptr<CpuMesh> sphereMesh(MeshCache& cache) {
-    if(!cache.unitSphere) {
+    if(!cache.unitSphere) cache.unitSphere = buildCpuMesh(SimTK::PolygonalMesh::createSphereMesh(1.0, 2));
-        cache.unitSphere = buildCpuMesh(SimTK::PolygonalMesh::createSphereMesh(1.0, 2));
-    }
     return cache.unitSphere;
 }
 
@@ -149,12 +147,65 @@ std::shared_ptr<CpuMesh> cylinderMesh(MeshCache& cache) {
 }
 
 std::shared_ptr<CpuMesh> brickMesh(MeshCache& cache) {
-    if(!cache.unitBrick) {
+    if(!cache.unitBrick) cache.unitBrick = buildCpuMesh(SimTK::PolygonalMesh::createBrickMesh(SimTK::Vec3(1.0, 1.0, 1.0), 1));
-        cache.unitBrick = buildCpuMesh(SimTK::PolygonalMesh::createBrickMesh(SimTK::Vec3(1.0, 1.0, 1.0), 1));
-    }
     return cache.unitBrick;
 }
 
+std::shared_ptr<CpuMesh> torusMesh(MeshCache& cache, const Float ringRadius, const Float tubeRadius) {
+    const std::string key = "torus:" + std::to_string(ringRadius) + ":" + std::to_string(tubeRadius);
+    if(const auto found = cache.meshes.find(key); found != cache.meshes.end()) return found->second;
+
+    constexpr Int RingSegments = 40;
+    constexpr Int TubeSegments = 20;
+
+    auto cpu = std::make_shared<CpuMesh>();
+    cpu->vertices.reserve(std::size_t(RingSegments*TubeSegments));
+    cpu->indices.reserve(std::size_t(RingSegments*TubeSegments*6));
+
+    bool hasBounds = false;
+    Range3D bounds;
+
+    for(Int ring = 0; ring != RingSegments; ++ring) {
+        const Float u = TwoPi*Float(ring)/Float(RingSegments);
+        const Float cu = std::cos(u);
+        const Float su = std::sin(u);
+
+        for(Int tube = 0; tube != TubeSegments; ++tube) {
+            const Float v = TwoPi*Float(tube)/Float(TubeSegments);
+            const Float cv = std::cos(v);
+            const Float sv = std::sin(v);
+
+            const Float radial = ringRadius + tubeRadius*cv;
+            const Vector3 position{radial*cu, radial*su, tubeRadius*sv};
+            const Vector3 normal = Vector3{cv*cu, cv*su, sv}.normalized();
+            cpu->vertices.push_back({position, normal});
+
+            const Range3D point = Range3D::fromSize(position, {});
+            if(hasBounds) bounds = Math::join(bounds, point);
+            else bounds = point;
+            hasBounds = true;
+        }
+    }
+
+    const auto vertexIndex = [](const Int ring, const Int tube) {
+        return UnsignedInt((ring%RingSegments)*TubeSegments + (tube%TubeSegments));
+    };
+
+    for(Int ring = 0; ring != RingSegments; ++ring) {
+        for(Int tube = 0; tube != TubeSegments; ++tube) {
+            const UnsignedInt a = vertexIndex(ring, tube);
+            const UnsignedInt b = vertexIndex(ring + 1, tube);
+            const UnsignedInt c = vertexIndex(ring + 1, tube + 1);
+            const UnsignedInt d = vertexIndex(ring, tube + 1);
+            cpu->indices.insert(cpu->indices.end(), {a, b, c, a, c, d});
+        }
+    }
+
+    cpu->bounds = bounds;
+    cache.meshes.emplace(key, cpu);
+    return cpu;
+}
+
 std::shared_ptr<CpuMesh> resolveMeshFile(MeshCache& cache, const OpenSim::Model& model, const std::string& meshFile) {
     bool isAbsolute = false;
     SimTK::Array_<std::string> attempts;
@@ -163,7 +214,7 @@ std::shared_ptr<CpuMesh> resolveMeshFile(MeshCache& cache, const OpenSim::Model&
     }
 
     const std::string resolved = attempts.back();
-    if(auto found = cache.meshes.find(resolved); found != cache.meshes.end()) return found->second;
+    if(const auto found = cache.meshes.find(resolved); found != cache.meshes.end()) return found->second;
 
     SimTK::PolygonalMesh mesh;
     mesh.loadFile(resolved);
@@ -181,24 +232,26 @@ GeometrySpec makeGeometrySpec(MeshCache& cache, const OpenSim::Model& model, con
     GeometrySpec spec;
     spec.name = geometry.getName();
     spec.color = toColor(geometry.get_Appearance());
-    spec.scale = geometryScale(geometry);
+    spec.transform = Matrix4::scaling(geometryScale(geometry));
 
     if(const auto* mesh = dynamic_cast<const OpenSim::Mesh*>(&geometry)) {
         spec.mesh = resolveMeshFile(cache, model, mesh->getGeometryFilename());
     } else if(const auto* sphere = dynamic_cast<const OpenSim::Sphere*>(&geometry)) {
         spec.mesh = sphereMesh(cache);
-        spec.scale *= Vector3{Float(sphere->get_radius())};
+        spec.transform = spec.transform*Matrix4::scaling(Vector3{Float(sphere->get_radius())});
     } else if(const auto* cylinder = dynamic_cast<const OpenSim::Cylinder*>(&geometry)) {
         spec.mesh = cylinderMesh(cache);
-        spec.scale *= Vector3{Float(cylinder->get_radius()), Float(cylinder->get_half_height()), Float(cylinder->get_radius())};
+        spec.transform = spec.transform*Matrix4::scaling(Vector3{Float(cylinder->get_radius()), Float(cylinder->get_half_height()), Float(cylinder->get_radius())});
     } else if(const auto* brick = dynamic_cast<const OpenSim::Brick*>(&geometry)) {
         spec.mesh = brickMesh(cache);
         const SimTK::Vec3 half = brick->get_half_lengths();
-        spec.scale *= Vector3{Float(half[0]), Float(half[1]), Float(half[2])};
+        spec.transform = spec.transform*Matrix4::scaling(Vector3{Float(half[0]), Float(half[1]), Float(half[2])});
     } else if(const auto* ellipsoid = dynamic_cast<const OpenSim::Ellipsoid*>(&geometry)) {
         spec.mesh = sphereMesh(cache);
         const SimTK::Vec3 radii = ellipsoid->get_radii();
-        spec.scale *= Vector3{Float(radii[0]), Float(radii[1]), Float(radii[2])};
+        spec.transform = spec.transform*Matrix4::scaling(Vector3{Float(radii[0]), Float(radii[1]), Float(radii[2])});
+    } else if(const auto* torus = dynamic_cast<const OpenSim::Torus*>(&geometry)) {
+        spec.mesh = torusMesh(cache, Float(torus->get_ring_radius()), Float(torus->get_cross_section()));
     } else {
         throw std::runtime_error("Unsupported geometry type: " + geometry.getConcreteClassName());
     }
@@ -206,7 +259,38 @@ GeometrySpec makeGeometrySpec(MeshCache& cache, const OpenSim::Model& model, con
     return spec;
 }
 
-Matrix4 frameMatrix(const Magnum::Vector3& translation, const Magnum::Quaternion& rotation) {
+GeometrySpec makeWrapGeometrySpec(MeshCache& cache, const OpenSim::WrapObject& wrap) {
+    GeometrySpec spec;
+    spec.name = wrap.getName();
+    spec.color = toColor(wrap.get_Appearance());
+    spec.layer = GeometryLayer::Wrap;
+
+    const Matrix4 localTransform = toMatrix4(wrap.getTransform());
+
+    if(const auto* sphere = dynamic_cast<const OpenSim::WrapSphere*>(&wrap)) {
+        spec.mesh = sphereMesh(cache);
+        spec.transform = localTransform*Matrix4::scaling(Vector3{Float(sphere->getRadius())});
+    } else if(const auto* cylinder = dynamic_cast<const OpenSim::WrapCylinder*>(&wrap)) {
+        spec.mesh = cylinderMesh(cache);
+        spec.transform =
+            localTransform*
+            Matrix4::rotationX(90.0_degf)*
+            Matrix4::scaling(Vector3{Float(cylinder->get_radius()), Float(cylinder->get_length()*0.5), Float(cylinder->get_radius())});
+    } else if(const auto* ellipsoid = dynamic_cast<const OpenSim::WrapEllipsoid*>(&wrap)) {
+        spec.mesh = sphereMesh(cache);
+        const SimTK::Vec3 radii = ellipsoid->getRadii();
+        spec.transform = localTransform*Matrix4::scaling(Vector3{Float(radii[0]), Float(radii[1]), Float(radii[2])});
+    } else if(const auto* torus = dynamic_cast<const OpenSim::WrapTorus*>(&wrap)) {
+        spec.mesh = torusMesh(cache, Float(torus->getOuterRadius()), Float(torus->getInnerRadius()));
+        spec.transform = localTransform;
+    } else {
+        throw std::runtime_error("Unsupported wrap geometry type: " + wrap.getConcreteClassName());
+    }
+
+    return spec;
+}
+
+Matrix4 frameMatrix(const Vector3& translation, const Quaternion& rotation) {
     return Matrix4::from(rotation.toMatrix(), translation);
 }
 
@@ -240,6 +324,47 @@ std::vector<std::string> geometrySearchDirsForModel(const std::string& modelPath
     return ordered;
 }
 
+std::string pathName(const OpenSim::GeometryPath& path) {
+    if(path.hasOwner() && !path.getOwner().getName().empty()) return path.getOwner().getName();
+    if(!path.getName().empty()) return path.getName();
+    return path.getAbsolutePathString();
+}
+
+std::vector<Vector3> pathPolyline(const OpenSim::Model& model,
+                                  const OpenSim::GeometryPath& path,
+                                  const SimTK::State& state) {
+    const auto& currentPath = path.getCurrentPath(state);
+    if(currentPath.getSize() < 2) return {};
+
+    std::vector<Vector3> polyline;
+    polyline.reserve(std::size_t(currentPath.getSize())*4);
+    polyline.push_back(toVector3(currentPath[0]->getLocationInGround(state)));
+
+    for(int i = 1; i < currentPath.getSize(); ++i) {
+        const auto* wrapPoint = dynamic_cast<const OpenSim::PathWrapPoint*>(currentPath[i]);
+        if(wrapPoint) {
+            const auto& wrapPath = wrapPoint->getWrapPath(state);
+            for(int j = 0; j < wrapPath.getSize(); ++j) {
+                const SimTK::Vec3 groundPoint =
+                    wrapPoint->getParentFrame().findStationLocationInAnotherFrame(
+                        state, wrapPath[j], model.getGround());
+                polyline.push_back(toVector3(groundPoint));
+            }
+        } else {
+            polyline.push_back(toVector3(currentPath[i]->getLocationInGround(state)));
+        }
+    }
+
+    return polyline;
+}
+
+void expandBoundsWithPoint(Range3D& bounds, bool& hasBounds, const Vector3& point) {
+    const Range3D rangePoint = Range3D::fromSize(point, {});
+    if(hasBounds) bounds = Math::join(bounds, rangePoint);
+    else bounds = rangePoint;
+    hasBounds = true;
+}
+
 }
 
 LoadedScene loadScene(const std::string& modelPath,
@@ -259,7 +384,8 @@ LoadedScene loadScene(const std::string& modelPath,
 
     OpenSim::Storage motionAsStates;
     model.formStateStorage(motion, motionAsStates, false);
-    const OpenSim::StatesTrajectory trajectory = OpenSim::StatesTrajectory::createFromStatesStorage(model, motionAsStates, true, true);
+    const OpenSim::StatesTrajectory trajectory =
+        OpenSim::StatesTrajectory::createFromStatesStorage(model, motionAsStates, true, true);
 
     MeshCache meshCache;
     LoadedScene scene;
@@ -267,27 +393,44 @@ LoadedScene loadScene(const std::string& modelPath,
     scene.motionName = motion.getName().empty() ? motionPath : motion.getName();
 
     std::vector<const OpenSim::PhysicalFrame*> framePtrs;
-    for(const OpenSim::PhysicalFrame& frame: model.getComponentList<OpenSim::PhysicalFrame>()) {
+    std::unordered_map<const OpenSim::PhysicalFrame*, std::size_t> frameIndices;
-        if(frame.getProperty_attached_geometry().size() == 0) continue;
+    const auto ensureFrameTrack = [&](const OpenSim::PhysicalFrame& frame) -> FrameTrack& {
+        if(const auto found = frameIndices.find(&frame); found != frameIndices.end()) {
+            return scene.frames[found->second];
+        }
 
         FrameTrack track;
         track.name = frame.getName();
+        framePtrs.push_back(&frame);
+        scene.frames.push_back(std::move(track));
+        const std::size_t index = scene.frames.size() - 1;
+        frameIndices.emplace(&frame, index);
+        return scene.frames[index];
+    };
+
+    for(const OpenSim::PhysicalFrame& frame: model.getComponentList<OpenSim::PhysicalFrame>()) {
         for(int i = 0; i != frame.getProperty_attached_geometry().size(); ++i) {
             const OpenSim::Geometry& geometry = frame.get_attached_geometry(i);
             if(!geometry.get_Appearance().get_visible()) continue;
             try {
-                track.geometries.push_back(makeGeometrySpec(meshCache, model, geometry));
+                ensureFrameTrack(frame).geometries.push_back(makeGeometrySpec(meshCache, model, geometry));
             } catch(const std::exception& error) {
                 std::cerr << "Warning: skipping geometry '" << geometry.getName()
                           << "' on frame '" << frame.getName() << "': "
                           << error.what() << '\n';
-                continue;
             }
         }
-        if(track.geometries.empty()) continue;
+    }
 
-        framePtrs.push_back(&frame);
+    for(const OpenSim::WrapObject& wrap: model.getComponentList<OpenSim::WrapObject>()) {
-        scene.frames.push_back(std::move(track));
+        if(!wrap.get_Appearance().get_visible()) continue;
+        try {
+            ensureFrameTrack(wrap.getFrame()).geometries.push_back(makeWrapGeometrySpec(meshCache, wrap));
+        } catch(const std::exception& error) {
+            std::cerr << "Warning: skipping wrap geometry '" << wrap.getName()
+                      << "' on frame '" << wrap.getFrame().getName() << "': "
+                      << error.what() << '\n';
+        }
     }
 
     std::vector<const OpenSim::Marker*> markerPtrs;
@@ -301,12 +444,25 @@ LoadedScene loadScene(const std::string& modelPath,
         scene.markers.push_back(std::move(track));
     }
 
+    std::vector<const OpenSim::GeometryPath*> pathPtrs;
+    for(const OpenSim::GeometryPath& path: model.getComponentList<OpenSim::GeometryPath>()) {
+        if(!path.isVisualPath() || !path.get_Appearance().get_visible()) continue;
+        PathTrack track;
+        track.name = pathName(path);
+        pathPtrs.push_back(&path);
+        scene.paths.push_back(std::move(track));
+    }
+
     scene.times.reserve(trajectory.getSize());
     for(FrameTrack& frame: scene.frames) {
         frame.translations.reserve(trajectory.getSize());
         frame.rotations.reserve(trajectory.getSize());
     }
     for(MarkerTrack& marker: scene.markers) marker.positions.reserve(trajectory.getSize());
+    for(PathTrack& path: scene.paths) {
+        path.colors.reserve(trajectory.getSize());
+        path.polylines.reserve(trajectory.getSize());
+    }
 
     bool hasSceneBounds = false;
 
@@ -324,8 +480,7 @@ LoadedScene loadScene(const std::string& modelPath,
 
             const Matrix4 world = frameMatrix(translation, rotation);
             for(const GeometrySpec& geometry: scene.frames[i].geometries) {
-                const Range3D scaled = scaledBounds(geometry.mesh->bounds, geometry.scale);
+                const Range3D worldBounds = transformBounds(geometry.mesh->bounds, world*geometry.transform);
-                const Range3D worldBounds = transformBounds(scaled, world);
                 if(hasSceneBounds) scene.initialBounds = Math::join(scene.initialBounds, worldBounds);
                 else scene.initialBounds = worldBounds;
                 hasSceneBounds = true;
@@ -335,15 +490,26 @@ LoadedScene loadScene(const std::string& modelPath,
         for(std::size_t i = 0; i != markerPtrs.size(); ++i) {
             const Vector3 position = toVector3(markerPtrs[i]->getLocationInGround(state));
             scene.markers[i].positions.push_back(position);
-            const Range3D point = Range3D::fromSize(position, {});
+            expandBoundsWithPoint(scene.initialBounds, hasSceneBounds, position);
-            if(hasSceneBounds) scene.initialBounds = Math::join(scene.initialBounds, point);
+        }
-            else scene.initialBounds = point;
+
-            hasSceneBounds = true;
+        for(std::size_t i = 0; i != pathPtrs.size(); ++i) {
+            std::vector<Vector3> polyline = pathPolyline(model, *pathPtrs[i], state);
+            if(polyline.size() < 2) {
+                scene.paths[i].polylines.emplace_back();
+                scene.paths[i].colors.push_back(pathColor(*pathPtrs[i], state));
+                continue;
+            }
+
+            scene.paths[i].maxPointCount = Math::max(scene.paths[i].maxPointCount, polyline.size());
+            scene.paths[i].colors.push_back(pathColor(*pathPtrs[i], state));
+            for(const Vector3& point: polyline) expandBoundsWithPoint(scene.initialBounds, hasSceneBounds, point);
+            scene.paths[i].polylines.push_back(std::move(polyline));
         }
     }
 
-    if(scene.frames.empty() && scene.markers.empty()) {
+    if(scene.frames.empty() && scene.markers.empty() && scene.paths.empty()) {
-        throw std::runtime_error("The model did not produce any renderable body geometry or markers.");
+        throw std::runtime_error("The model did not produce any renderable body geometry, markers, or geometry paths.");
     }
 
     return scene;

src/OpenSimLoader.hpp

@@ -2,9 +2,11 @@
 
 #include <Magnum/Math/Color.h>
 #include <Magnum/Math/Range.h>
+#include <Magnum/Math/Matrix4.h>
 #include <Magnum/Math/Vector3.h>
 #include <Magnum/Math/Quaternion.h>
 
+#include <cstddef>
 #include <memory>
 #include <string>
 #include <vector>
@@ -22,11 +24,17 @@ struct CpuMesh {
     Magnum::Range3D bounds;
 };
 
+enum class GeometryLayer {
+    Body,
+    Wrap,
+};
+
 struct GeometrySpec {
     std::shared_ptr<CpuMesh> mesh;
-    Magnum::Vector3 scale{1.0f};
+    Magnum::Matrix4 transform{Magnum::Math::IdentityInit};
     Magnum::Color4 color{1.0f};
     std::string name;
+    GeometryLayer layer{GeometryLayer::Body};
 };
 
 struct FrameTrack {
@@ -42,12 +50,20 @@ struct MarkerTrack {
     std::vector<Magnum::Vector3> positions;
 };
 
+struct PathTrack {
+    std::string name;
+    std::size_t maxPointCount{};
+    std::vector<Magnum::Color4> colors;
+    std::vector<std::vector<Magnum::Vector3>> polylines;
+};
+
 struct LoadedScene {
     std::string modelName;
     std::string motionName;
     std::vector<float> times;
     std::vector<FrameTrack> frames;
     std::vector<MarkerTrack> markers;
+    std::vector<PathTrack> paths;
     Magnum::Range3D initialBounds;
 };
 

src/ViewerApp.cpp

@@ -15,7 +15,6 @@
 
 #include <algorithm>
 #include <cmath>
-#include <limits>
 #include <stdexcept>
 #include <unordered_map>
 
@@ -26,6 +25,11 @@ namespace osim_viewer {
 
 namespace {
 
+constexpr Float MarkerRadius = 0.01f;
+constexpr Float PathPointRadius = 0.005f;
+constexpr Float PathSegmentRadius = 0.0025f;
+constexpr Float SegmentEpsilon = 1.0e-5f;
+
 Vector3 toVector3(const SimTK::Vec3& value) {
     return {Float(value[0]), Float(value[1]), Float(value[2])};
 }
@@ -50,9 +54,9 @@ std::shared_ptr<CpuMesh> buildCpuMesh(const SimTK::PolygonalMesh& mesh) {
             normals[a] += normal;
             normals[b] += normal;
             normals[c] += normal;
-            cpu->indices.push_back(Magnum::UnsignedInt(a));
+            cpu->indices.push_back(UnsignedInt(a));
-            cpu->indices.push_back(Magnum::UnsignedInt(b));
+            cpu->indices.push_back(UnsignedInt(b));
-            cpu->indices.push_back(Magnum::UnsignedInt(c));
+            cpu->indices.push_back(UnsignedInt(c));
         }
     }
 
@@ -117,23 +121,83 @@ std::shared_ptr<CpuMesh> buildBoxMesh() {
     return cpu;
 }
 
+Color4 lerpColor(const Color4& a, const Color4& b, const Float alpha) {
+    return {
+        Math::lerp(a.r(), b.r(), alpha),
+        Math::lerp(a.g(), b.g(), alpha),
+        Math::lerp(a.b(), b.b(), alpha),
+        Math::lerp(a.a(), b.a(), alpha)
+    };
+}
+
+Quaternion rotationFromYAxis(const Vector3& direction) {
+    const Vector3 normalizedDirection = direction.normalized();
+    const Float dot = Math::clamp(Math::dot(Vector3::yAxis(), normalizedDirection), -1.0f, 1.0f);
+    if(dot > 0.9999f) return Quaternion{};
+    if(dot < -0.9999f) return Quaternion::rotation(180.0_degf, Vector3::xAxis());
+
+    const Vector3 axis = Math::cross(Vector3::yAxis(), normalizedDirection).normalized();
+    return Quaternion::rotation(Rad{std::acos(dot)}, axis).normalized();
+}
+
+Matrix4 segmentTransform(const Vector3& start, const Vector3& end) {
+    const Vector3 delta = end - start;
+    const Float length = delta.length();
+    const Quaternion rotation = rotationFromYAxis(delta);
+    return Matrix4::translation((start + end)*0.5f)*
+           Matrix4::from(rotation.toMatrix(), {})*
+           Matrix4::scaling(Vector3{PathSegmentRadius, length*0.5f, PathSegmentRadius});
+}
+
+std::vector<Vector3> interpolatedPathPolyline(const PathTrack& track,
+                                              const std::size_t sample,
+                                              const std::size_t next,
+                                              const Float alpha) {
+    const auto& samplePoints = track.polylines[sample];
+    const auto& nextPoints = track.polylines[next];
+    if(sample == next) return samplePoints;
+    if(samplePoints.size() != nextPoints.size()) return alpha < 0.5f ? samplePoints : nextPoints;
+
+    std::vector<Vector3> out(samplePoints.size());
+    for(std::size_t i = 0; i != samplePoints.size(); ++i) {
+        out[i] = Math::lerp(samplePoints[i], nextPoints[i], alpha);
+    }
+    return out;
+}
+
+Color4 interpolatedPathColor(const PathTrack& track,
+                             const std::size_t sample,
+                             const std::size_t next,
+                             const Float alpha) {
+    const auto& samplePoints = track.polylines[sample];
+    const auto& nextPoints = track.polylines[next];
+    if(sample == next) return track.colors[sample];
+    if(samplePoints.size() != nextPoints.size()) return alpha < 0.5f ? track.colors[sample] : track.colors[next];
+    return lerpColor(track.colors[sample], track.colors[next], alpha);
+}
+
 }
 
 ViewerApp::MeshDrawable::MeshDrawable(Object3D& object,
                                       Shaders::PhongGL& shader,
                                       GpuMesh& mesh,
-                                      Color4 color,
+                                      std::shared_ptr<Color4> color,
+                                      std::shared_ptr<bool> visible,
                                       DrawableGroup3D& drawables):
     Drawable3D{object, &drawables},
     _shader{shader},
     _mesh{mesh},
-    _color{color} {}
+    _color{std::move(color)},
+    _visible{std::move(visible)} {}
 
 void ViewerApp::MeshDrawable::draw(const Matrix4& transformation, Camera3D& camera) {
+    if(_visible && !*_visible) return;
+
+    const Color4 color = _color ? *_color : Color4{1.0f};
     _shader
         .setLightPositions({{4.0f, 6.0f, 4.0f, 0.0f}})
-        .setAmbientColor(Color4{_color.rgb()*0.35f, _color.a()})
+        .setAmbientColor(Color4{color.rgb()*0.35f, color.a()})
-        .setDiffuseColor(_color)
+        .setDiffuseColor(color)
         .setTransformationMatrix(transformation)
         .setNormalMatrix(transformation.normalMatrix())
         .setProjectionMatrix(camera.projectionMatrix())
@@ -209,12 +273,19 @@ std::shared_ptr<ViewerApp::GpuMesh> ViewerApp::uploadMesh(const CpuMesh& mesh) c
 
 void ViewerApp::createSceneObjects() {
     std::unordered_map<const CpuMesh*, std::shared_ptr<GpuMesh>> meshMap;
-    auto markerCpu = buildCpuMesh(SimTK::PolygonalMesh::createSphereMesh(1.0, 1));
+
-    _markerMesh = uploadMesh(*markerCpu);
+    auto sphereCpu = buildCpuMesh(SimTK::PolygonalMesh::createSphereMesh(1.0, 1));
+    _markerMesh = uploadMesh(*sphereCpu);
     _gpuMeshes.push_back(_markerMesh);
+
+    auto cylinderCpu = buildCpuMesh(SimTK::PolygonalMesh::createCylinderMesh(SimTK::YAxis, 1.0, 1.0, 2));
+    _cylinderMesh = uploadMesh(*cylinderCpu);
+    _gpuMeshes.push_back(_cylinderMesh);
+
     auto boxCpu = buildBoxMesh();
     _boxMesh = uploadMesh(*boxCpu);
     _gpuMeshes.push_back(_boxMesh);
+
     _groundGridMesh = MeshTools::compile(Primitives::grid3DWireframe({20, 20}));
     _originWireBoxMesh = MeshTools::compile(Primitives::cubeWireframe());
 
@@ -234,10 +305,18 @@ void ViewerApp::createSceneObjects() {
             }
 
             auto geometryObject = std::make_unique<Object3D>(frame.object);
-            geometryObject->setTransformation(Matrix4::scaling(geometry.scale));
+            geometryObject->setTransformation(geometry.transform);
-            auto drawable = std::make_unique<MeshDrawable>(*geometryObject, _shader, *gpuMesh, geometry.color, _drawables);
 
-            frame.geometries.push_back({geometryObject.get(), geometry.color});
+            auto color = std::make_shared<Color4>(geometry.color);
+            auto drawable = std::make_unique<MeshDrawable>(
+                *geometryObject,
+                _shader,
+                *gpuMesh,
+                std::move(color),
+                nullptr,
+                geometry.layer == GeometryLayer::Wrap ? _wrapDrawables : _bodyDrawables);
+
+            if(geometry.layer == GeometryLayer::Wrap) ++_wrapGeometryCount;
             _objectStorage.push_back(std::move(geometryObject));
             _drawableStorage.push_back(std::move(drawable));
         }
@@ -246,16 +325,42 @@ void ViewerApp::createSceneObjects() {
         _objectStorage.push_back(std::move(frameObject));
     }
 
-    constexpr Float MarkerRadius = 0.01f;
     for(const MarkerTrack& markerTrack: _sceneData.markers) {
         auto markerObject = std::make_unique<Object3D>(&_scene);
         markerObject->setTransformation(Matrix4::scaling(Vector3{MarkerRadius}));
-        auto drawable = std::make_unique<MeshDrawable>(*markerObject, _shader, *_markerMesh, markerTrack.color, _drawables);
+        auto color = std::make_shared<Color4>(markerTrack.color);
-        _markers.push_back({markerObject.get(), markerTrack.color});
+        auto drawable = std::make_unique<MeshDrawable>(*markerObject, _shader, *_markerMesh, std::move(color), nullptr, _markerDrawables);
+        _markers.push_back({markerObject.get()});
         _objectStorage.push_back(std::move(markerObject));
         _drawableStorage.push_back(std::move(drawable));
     }
 
+    for(const PathTrack& pathTrack: _sceneData.paths) {
+        ScenePath path;
+        path.color = std::make_shared<Color4>(pathTrack.colors.empty() ? Color4{0.85f, 0.2f, 0.2f, 1.0f} : pathTrack.colors.front());
+
+        const std::size_t segmentCount = pathTrack.maxPointCount > 1 ? pathTrack.maxPointCount - 1 : 0;
+        for(std::size_t i = 0; i != segmentCount; ++i) {
+            auto object = std::make_unique<Object3D>(&_scene);
+            auto visible = std::make_shared<bool>(false);
+            auto drawable = std::make_unique<MeshDrawable>(*object, _shader, *_cylinderMesh, path.color, visible, _pathDrawables);
+            path.segments.push_back({object.get(), visible});
+            _objectStorage.push_back(std::move(object));
+            _drawableStorage.push_back(std::move(drawable));
+        }
+
+        for(std::size_t i = 0; i != pathTrack.maxPointCount; ++i) {
+            auto object = std::make_unique<Object3D>(&_scene);
+            auto visible = std::make_shared<bool>(false);
+            auto drawable = std::make_unique<MeshDrawable>(*object, _shader, *_markerMesh, path.color, visible, _pathPointDrawables);
+            path.points.push_back({object.get(), visible});
+            _objectStorage.push_back(std::move(object));
+            _drawableStorage.push_back(std::move(drawable));
+        }
+
+        _paths.push_back(std::move(path));
+    }
+
     createReferenceObjects();
 }
 
@@ -281,7 +386,13 @@ void ViewerApp::createReferenceObjects() {
     for(const AxisSpec& axis: axes) {
         auto object = std::make_unique<Object3D>(&_scene);
         object->setTransformation(Matrix4::translation(axis.translation)*Matrix4::scaling(axis.scale));
-        auto drawable = std::make_unique<MeshDrawable>(*object, _shader, *_boxMesh, axis.color, _drawables);
+        auto drawable = std::make_unique<MeshDrawable>(
+            *object,
+            _shader,
+            *_boxMesh,
+            std::make_shared<Color4>(axis.color),
+            nullptr,
+            _referenceDrawables);
         _drawableStorage.push_back(std::move(drawable));
         _objectStorage.push_back(std::move(object));
     }
@@ -289,7 +400,7 @@ void ViewerApp::createReferenceObjects() {
     auto originObject = std::make_unique<Object3D>(&_scene);
     originObject->setTransformation(Matrix4::scaling(Vector3{originMarkerScale}));
     auto originDrawable = std::make_unique<FlatDrawable>(
-        *originObject, _flatShader, _originWireBoxMesh, Color4{0.82f, 0.84f, 0.88f, 1.0f}, _drawables);
+        *originObject, _flatShader, _originWireBoxMesh, Color4{0.82f, 0.84f, 0.88f, 1.0f}, _referenceDrawables);
     _drawableStorage.push_back(std::move(originDrawable));
     _objectStorage.push_back(std::move(originObject));
 
@@ -298,7 +409,7 @@ void ViewerApp::createReferenceObjects() {
         Matrix4::rotationX(90.0_degf)*
         Matrix4::scaling(Vector3{groundScale}));
     auto groundDrawable = std::make_unique<FlatDrawable>(
-        *groundObject, _flatShader, _groundGridMesh, Color4{0.42f, 0.45f, 0.48f, 1.0f}, _drawables);
+        *groundObject, _flatShader, _groundGridMesh, Color4{0.42f, 0.45f, 0.48f, 1.0f}, _referenceDrawables);
     _drawableStorage.push_back(std::move(groundDrawable));
     _objectStorage.push_back(std::move(groundObject));
 }
@@ -352,7 +463,33 @@ void ViewerApp::updateSceneAtCurrentTime() {
 
     for(std::size_t i = 0; i != _markers.size(); ++i) {
         const Vector3 position = Math::lerp(_sceneData.markers[i].positions[sample], _sceneData.markers[i].positions[next], alpha);
-        _markers[i].object->setTransformation(Matrix4::translation(position)*Matrix4::scaling(Vector3{0.01f}));
+        _markers[i].object->setTransformation(Matrix4::translation(position)*Matrix4::scaling(Vector3{MarkerRadius}));
+    }
+
+    for(std::size_t i = 0; i != _paths.size(); ++i) {
+        std::vector<Vector3> polyline = interpolatedPathPolyline(_sceneData.paths[i], sample, next, alpha);
+        *_paths[i].color = interpolatedPathColor(_sceneData.paths[i], sample, next, alpha);
+
+        for(std::size_t segmentIndex = 0; segmentIndex != _paths[i].segments.size(); ++segmentIndex) {
+            const bool activeSegment =
+                segmentIndex + 1 < polyline.size() &&
+                (polyline[segmentIndex + 1] - polyline[segmentIndex]).length() > SegmentEpsilon;
+            *_paths[i].segments[segmentIndex].visible = activeSegment;
+            if(activeSegment) {
+                _paths[i].segments[segmentIndex].object->setTransformation(
+                    segmentTransform(polyline[segmentIndex], polyline[segmentIndex + 1]));
+            }
+        }
+
+        for(std::size_t pointIndex = 0; pointIndex != _paths[i].points.size(); ++pointIndex) {
+            const bool activePoint = pointIndex < polyline.size();
+            *_paths[i].points[pointIndex].visible = activePoint;
+            if(activePoint) {
+                _paths[i].points[pointIndex].object->setTransformation(
+                    Matrix4::translation(polyline[pointIndex])*
+                    Matrix4::scaling(Vector3{PathPointRadius}));
+            }
+        }
     }
 }
 
@@ -372,6 +509,9 @@ void ViewerApp::drawUi() {
     }
     ImGui::SliderFloat("Speed", &_playbackSpeed, 0.1f, 4.0f, "%.2fx");
     ImGui::Checkbox("Loop", &_loopPlayback);
+    ImGui::Checkbox("Paths", &_showPaths);
+    ImGui::Checkbox("Path points", &_showPathPoints);
+    ImGui::Checkbox("Wrap geometry", &_showWrapGeometry);
     if(ImGui::Checkbox("Reverse mouse Y", &_options.reverseMouseY)) {
         _orbit.setInvertY(!_options.reverseMouseY);
     }
@@ -383,7 +523,8 @@ void ViewerApp::drawUi() {
         }
     }
     ImGui::Text("Time %.3f / %.3f", _currentTime, _sceneData.times.empty() ? 0.0f : _sceneData.times.back());
-    ImGui::Text("Frames %zu  Markers %zu", _sceneData.frames.size(), _sceneData.markers.size());
+    ImGui::Text("Frames %zu  Markers %zu  Paths %zu  Wraps %zu",
+                _sceneData.frames.size(), _sceneData.markers.size(), _sceneData.paths.size(), _wrapGeometryCount);
     ImGui::TextUnformatted("LMB orbit  Shift+LMB/RMB/MMB pan  wheel zoom  F frame");
     ImGui::End();
 }
@@ -447,7 +588,14 @@ void ViewerApp::drawEvent() {
     _cameraObject.setTransformation(_orbit.cameraTransform());
 
     GL::defaultFramebuffer.clear(GL::FramebufferClear::Color | GL::FramebufferClear::Depth);
-    _camera.draw(_drawables);
+    GL::Renderer::enable(GL::Renderer::Feature::Blending);
+    _camera.draw(_bodyDrawables);
+    if(_showWrapGeometry) _camera.draw(_wrapDrawables);
+    _camera.draw(_markerDrawables);
+    if(_showPaths) _camera.draw(_pathDrawables);
+    if(_showPathPoints) _camera.draw(_pathPointDrawables);
+    _camera.draw(_referenceDrawables);
+    GL::Renderer::disable(GL::Renderer::Feature::Blending);
 
     _imgui.newFrame();
     if(ImGui::GetIO().WantTextInput && !isTextInputActive()) startTextInput();
@@ -490,11 +638,10 @@ void ViewerApp::keyPressEvent(KeyEvent& event) {
             rewindPlayback();
             event.setAccepted();
             return;
-        case Key::F: {
+        case Key::F:
             resetCameraToScene();
             event.setAccepted();
             return;
-        }
         case Key::LeftBracket:
             _playbackSpeed = Math::max(_playbackSpeed*0.5f, 0.1f);
             event.setAccepted();
@@ -517,8 +664,9 @@ void ViewerApp::pointerPressEvent(PointerEvent& event) {
     _lastPointerPosition = event.position();
     if(event.pointer() == Pointer::MouseLeft) {
         _dragMode = (event.modifiers() & Modifier::Shift) ? DragMode::Pan : DragMode::Orbit;
+    } else if(event.pointer() == Pointer::MouseRight || event.pointer() == Pointer::MouseMiddle) {
+        _dragMode = DragMode::Pan;
     }
-    else if(event.pointer() == Pointer::MouseRight || event.pointer() == Pointer::MouseMiddle) _dragMode = DragMode::Pan;
     event.setAccepted();
 }
 

src/ViewerApp.hpp

@@ -70,19 +70,23 @@ private:
         Magnum::Range3D bounds;
     };
 
-    struct SceneGeometry {
+    struct ScenePrimitive {
         Object3D* object{};
-        Magnum::Color4 color{1.0f};
+        std::shared_ptr<bool> visible;
     };
 
     struct SceneFrame {
         Object3D* object{};
-        std::vector<SceneGeometry> geometries;
     };
 
     struct SceneMarker {
         Object3D* object{};
-        Magnum::Color4 color{1.0f};
+    };
+
+    struct ScenePath {
+        std::shared_ptr<Magnum::Color4> color;
+        std::vector<ScenePrimitive> segments;
+        std::vector<ScenePrimitive> points;
     };
 
     class MeshDrawable final: public Drawable3D {
@@ -90,7 +94,8 @@ private:
         MeshDrawable(Object3D& object,
                      Magnum::Shaders::PhongGL& shader,
                      GpuMesh& mesh,
-                     Magnum::Color4 color,
+                     std::shared_ptr<Magnum::Color4> color,
+                     std::shared_ptr<bool> visible,
                      DrawableGroup3D& drawables);
 
     private:
@@ -98,7 +103,8 @@ private:
 
         Magnum::Shaders::PhongGL& _shader;
         GpuMesh& _mesh;
-        Magnum::Color4 _color;
+        std::shared_ptr<Magnum::Color4> _color;
+        std::shared_ptr<bool> _visible;
     };
 
     class FlatDrawable final: public Drawable3D {
@@ -141,7 +147,12 @@ private:
     Scene3D _scene;
     Object3D _cameraObject{&_scene};
     Camera3D _camera{_cameraObject};
-    DrawableGroup3D _drawables;
+    DrawableGroup3D _bodyDrawables;
+    DrawableGroup3D _wrapDrawables;
+    DrawableGroup3D _markerDrawables;
+    DrawableGroup3D _pathDrawables;
+    DrawableGroup3D _pathPointDrawables;
+    DrawableGroup3D _referenceDrawables;
 
     Magnum::Shaders::PhongGL _shader;
     Magnum::Shaders::FlatGL3D _flatShader;
@@ -150,16 +161,22 @@ private:
     std::vector<std::unique_ptr<Object3D>> _objectStorage;
     std::vector<SceneFrame> _frames;
     std::vector<SceneMarker> _markers;
+    std::vector<ScenePath> _paths;
 
     std::shared_ptr<GpuMesh> _markerMesh;
+    std::shared_ptr<GpuMesh> _cylinderMesh;
     std::shared_ptr<GpuMesh> _boxMesh;
     Magnum::GL::Mesh _groundGridMesh;
     Magnum::GL::Mesh _originWireBoxMesh;
+    std::size_t _wrapGeometryCount = 0;
 
     bool _isPlaying = false;
     bool _loopPlayback = true;
     float _playbackSpeed = 1.0f;
     float _currentTime = 0.0f;
+    bool _showPaths = true;
+    bool _showPathPoints = true;
+    bool _showWrapGeometry = true;
     std::chrono::steady_clock::time_point _lastTick;
 
     std::optional<Magnum::Vector2> _lastPointerPosition;