feat: visualize opensim muscle paths and wraps

Accumulate scene bounds from actual geometry and marker samples across the full trajectory instead of seeding Range3D with sentinel extrema and only joining against the first sample.

Why this loader change is necessary:

- Viewer camera framing and reference sizing derive from scene.initialBounds.

- The old code only incorporated sample==0 geometry and marker positions, so motions that travel away from the initial pose could be framed incorrectly.

- The old sentinel initialization depended on joining with artificial min/max values instead of real bounds.

What changed:

- Use first-real-bound initialization in scaledBounds(), transformBounds(), and buildCpuMesh().

- Accumulate scene.initialBounds across all trajectory samples in src/OpenSimLoader.cpp.

- Ignore generated runtime files imgui.ini and opensim.log in .gitignore.

Validation:

- Rebuilt successfully with: cmake --build build -j

.gitignore

@@ -45,3 +45,7 @@ compile_commands.json
 Makefile
 cmake_install.cmake
 Testing/
+
+# runtime files
+imgui.ini
+opensim.log

README.md

@@ -31,3 +31,7 @@ Sample run:
   /home/crosstyan/Code/opensim-core/OpenSim/Moco/Test/walk_gait1018_subject01.osim \
   /home/crosstyan/Code/opensim-core/OpenSim/Moco/Test/walk_gait1018_state_reference.mot
 ```
+
+## Notes
+
+- Motion troubleshooting notes: [`docs/motion-troubleshooting.md`](docs/motion-troubleshooting.md)

docs/motion-troubleshooting.md

@@ -0,0 +1,45 @@
+# Motion Troubleshooting
+
+## Bone Flips During Playback
+
+The viewer loads the `.mot` file through OpenSim, converts it to model states,
+and renders each body using `PhysicalFrame::getTransformInGround()`. It does
+not solve IK, rebuild bones from markers, or apply any custom joint logic.
+
+Because of that, a visible flip can come from two different places:
+
+1. The upstream motion data already contains a discontinuity.
+2. Playback interpolation between two valid sampled poses introduces an
+   artificial long-arc rotation.
+
+## Viewer Behavior
+
+Playback now uses shortest-path quaternion interpolation between sampled OpenSim
+poses. This avoids false in-between spins when adjacent quaternions have
+opposite signs but represent nearly the same orientation.
+
+If a flip is still visible after this fix, the sampled OpenSim motion itself is
+already discontinuous.
+
+## Upstream IK Failures
+
+For Sports2D / Pose2Sim / OpenSim pipelines, broken IK usually shows up as one
+or more of the following:
+
+- sudden large frame-to-frame jumps in joint coordinates
+- joint values snapping to model limits
+- marker RMS staying low while anatomically implausible body orientations appear
+- left/right ambiguity or bad depth reconstruction in the source TRC
+
+Sports2D ultimately delegates IK generation to Pose2Sim, which then calls
+`opensim.InverseKinematicsTool(...)`. If the generated `_ik.mot` already
+contains large discontinuities, the viewer is only displaying that failure.
+
+## Practical Check
+
+To separate viewer issues from upstream IK issues:
+
+1. Load the `.osim` and `.mot` in the viewer.
+2. Scrub near the suspicious frame while paused.
+3. If the sampled pose itself is wrong, the IK output is broken upstream.
+4. If only the in-between motion looks wrong, interpolation is the likely cause.

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,34 +44,31 @@ 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) {
-    const SimTK::Vec3 color = appearance.get_color();
-    return {Float(color[0]), Float(color[1]), Float(color[2]), Float(appearance.get_opacity())};
+Matrix4 toMatrix4(const SimTK::Transform& transform) {
+    return Matrix4::from(toQuaternion(transform.R()).toMatrix(), toVector3(transform.T()));
 }
 
-Range3D scaledBounds(const Range3D& bounds, const Vector3& scale) {
-    Range3D out;
-    out.min() = Vector3{std::numeric_limits<Float>::max()};
-    out.max() = Vector3{-std::numeric_limits<Float>::max()};
-    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()},
-        }) {
-        out = Math::join(out, Range3D::fromSize(corner*scale, {}));
+Color4 toColor(const SimTK::Vec3& color, const Float opacity = 1.0f) {
+    return {Float(color[0]), Float(color[1]), Float(color[2]), opacity};
+}
+
+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 out;
+    return toColor(path.get_Appearance());
 }
 
 Range3D transformBounds(const Range3D& bounds, const Matrix4& transform) {
+    bool hasBounds = false;
     Range3D out;
-    out.min() = Vector3{std::numeric_limits<Float>::max()};
-    out.max() = Vector3{-std::numeric_limits<Float>::max()};
     for(const Vector3& corner: {
             Vector3{bounds.min().x(), bounds.min().y(), bounds.min().z()},
             Vector3{bounds.min().x(), bounds.min().y(), bounds.max().z()},
@@ -76,7 +79,10 @@ Range3D transformBounds(const Range3D& bounds, const Matrix4& transform) {
             Vector3{bounds.max().x(), bounds.max().y(), bounds.min().z()},
             Vector3{bounds.max().x(), bounds.max().y(), bounds.max().z()},
         }) {
-        out = Math::join(out, Range3D::fromSize(transform.transformPoint(corner), {}));
+        const Range3D point = Range3D::fromSize(transform.transformPoint(corner), {});
+        if(hasBounds) out = Math::join(out, point);
+        else out = point;
+        hasBounds = true;
     }
     return out;
 }
@@ -102,36 +108,34 @@ 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 ac = positions[c] - positions[a];
-            const Vector3 faceNormal = Math::cross(ab, ac);
+            const Vector3 faceNormal = Math::cross(positions[b] - positions[a], positions[c] - positions[a]);
             normals[a] += faceNormal;
             normals[b] += faceNormal;
             normals[c] += faceNormal;
-            cpu->indices.push_back(Magnum::UnsignedInt(a));
-            cpu->indices.push_back(Magnum::UnsignedInt(b));
-            cpu->indices.push_back(Magnum::UnsignedInt(c));
+            cpu->indices.push_back(UnsignedInt(a));
+            cpu->indices.push_back(UnsignedInt(b));
+            cpu->indices.push_back(UnsignedInt(c));
         }
     }
 
+    bool hasBounds = false;
     Range3D bounds;
-    bounds.min() = Vector3{std::numeric_limits<Float>::max()};
-    bounds.max() = Vector3{-std::numeric_limits<Float>::max()};
 
     cpu->vertices.reserve(positions.size());
     for(std::size_t i = 0; i != positions.size(); ++i) {
         const Vector3 normal = normals[i].isZero() ? Vector3::yAxis() : normals[i].normalized();
         cpu->vertices.push_back({positions[i], normal});
-        bounds = Math::join(bounds, Range3D::fromSize(positions[i], {}));
+        const Range3D point = Range3D::fromSize(positions[i], {});
+        if(hasBounds) bounds = Math::join(bounds, point);
+        else bounds = point;
+        hasBounds = true;
     }
     cpu->bounds = bounds;
     return cpu;
 }
 
 std::shared_ptr<CpuMesh> sphereMesh(MeshCache& cache) {
-    if(!cache.unitSphere) {
-        cache.unitSphere = buildCpuMesh(SimTK::PolygonalMesh::createSphereMesh(1.0, 2));
-    }
+    if(!cache.unitSphere) cache.unitSphere = buildCpuMesh(SimTK::PolygonalMesh::createSphereMesh(1.0, 2));
     return cache.unitSphere;
 }
 
@@ -143,12 +147,65 @@ std::shared_ptr<CpuMesh> cylinderMesh(MeshCache& cache) {
 }
 
 std::shared_ptr<CpuMesh> brickMesh(MeshCache& cache) {
-    if(!cache.unitBrick) {
-        cache.unitBrick = buildCpuMesh(SimTK::PolygonalMesh::createBrickMesh(SimTK::Vec3(1.0, 1.0, 1.0), 1));
-    }
+    if(!cache.unitBrick) 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;
@@ -157,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);
@@ -175,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());
     }
@@ -200,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);
 }
 
@@ -234,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,
@@ -253,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;
@@ -261,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>()) {
-        if(frame.getProperty_attached_geometry().size() == 0) continue;
+    std::unordered_map<const OpenSim::PhysicalFrame*, std::size_t> frameIndices;
+    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);
-        scene.frames.push_back(std::move(track));
+    for(const OpenSim::WrapObject& wrap: model.getComponentList<OpenSim::WrapObject>()) {
+        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;
@@ -295,15 +444,27 @@ 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());
+    }
 
-    scene.initialBounds.min() = Vector3{std::numeric_limits<Float>::max()};
-    scene.initialBounds.max() = Vector3{-std::numeric_limits<Float>::max()};
+    bool hasSceneBounds = false;
 
     for(std::size_t sample = 0; sample != trajectory.getSize(); ++sample) {
         const SimTK::State& state = trajectory[sample];
@@ -317,24 +478,38 @@ LoadedScene loadScene(const std::string& modelPath,
             scene.frames[i].translations.push_back(translation);
             scene.frames[i].rotations.push_back(rotation);
 
-            if(sample == 0) {
             const Matrix4 world = frameMatrix(translation, rotation);
             for(const GeometrySpec& geometry: scene.frames[i].geometries) {
-                    const Range3D scaled = scaledBounds(geometry.mesh->bounds, geometry.scale);
-                    scene.initialBounds = Math::join(scene.initialBounds, transformBounds(scaled, world));
-                }
+                const Range3D worldBounds = transformBounds(geometry.mesh->bounds, world*geometry.transform);
+                if(hasSceneBounds) scene.initialBounds = Math::join(scene.initialBounds, worldBounds);
+                else scene.initialBounds = worldBounds;
+                hasSceneBounds = true;
             }
         }
 
         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);
-            if(sample == 0) scene.initialBounds = Math::join(scene.initialBounds, Range3D::fromSize(position, {}));
+            expandBoundsWithPoint(scene.initialBounds, hasSceneBounds, position);
+        }
+
+        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()) {
-        throw std::runtime_error("The model did not produce any renderable body geometry or markers.");
+    if(scene.frames.empty() && scene.markers.empty() && scene.paths.empty()) {
+        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(Magnum::UnsignedInt(b));
-            cpu->indices.push_back(Magnum::UnsignedInt(c));
+            cpu->indices.push_back(UnsignedInt(a));
+            cpu->indices.push_back(UnsignedInt(b));
+            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()})
-        .setDiffuseColor(_color)
+        .setAmbientColor(Color4{color.rgb()*0.35f, color.a()})
+        .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));
-            auto drawable = std::make_unique<MeshDrawable>(*geometryObject, _shader, *gpuMesh, geometry.color, _drawables);
+            geometryObject->setTransformation(geometry.transform);
 
-            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);
-        _markers.push_back({markerObject.get(), markerTrack.color});
+        auto color = std::make_shared<Color4>(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));
 }
@@ -343,13 +454,42 @@ void ViewerApp::updateSceneAtCurrentTime() {
 
     for(std::size_t i = 0; i != _frames.size(); ++i) {
         const Vector3 translation = Math::lerp(_sceneData.frames[i].translations[sample], _sceneData.frames[i].translations[next], alpha);
-        const Quaternion rotation = Math::slerp(_sceneData.frames[i].rotations[sample], _sceneData.frames[i].rotations[next], alpha).normalized();
+        const Quaternion rotation = Math::slerpShortestPath(
+            _sceneData.frames[i].rotations[sample],
+            _sceneData.frames[i].rotations[next],
+            alpha).normalized();
         _frames[i].object->setTransformation(makeFrameMatrix(translation, rotation));
     }
 
     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}));
+            }
+        }
     }
 }
 
@@ -369,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);
     }
@@ -380,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();
 }
@@ -444,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();
@@ -487,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();
@@ -514,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;