diff --git a/src/OpenSimLoader.cpp b/src/OpenSimLoader.cpp index 7853f9e..413bb4b 100644 --- a/src/OpenSimLoader.cpp +++ b/src/OpenSimLoader.cpp @@ -1,22 +1,25 @@ #include "OpenSimLoader.hpp" #include -#include #include +#include #include #include #include #include +#include +#include +#include +#include +#include #include #include #include -#include #include #include #include -#include #include #include #include @@ -24,11 +27,14 @@ #include 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) { - 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) { - bool hasBounds = false; - 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; +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) { @@ -106,15 +108,13 @@ std::shared_ptr 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)); } } @@ -135,9 +135,7 @@ std::shared_ptr buildCpuMesh(const SimTK::PolygonalMesh& mesh) { } std::shared_ptr 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; } @@ -149,12 +147,65 @@ std::shared_ptr cylinderMesh(MeshCache& cache) { } std::shared_ptr 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 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(); + 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 resolveMeshFile(MeshCache& cache, const OpenSim::Model& model, const std::string& meshFile) { bool isAbsolute = false; SimTK::Array_ attempts; @@ -163,7 +214,7 @@ std::shared_ptr 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(&geometry)) { spec.mesh = resolveMeshFile(cache, model, mesh->getGeometryFilename()); } else if(const auto* sphere = dynamic_cast(&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(&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(&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(&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(&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(&wrap)) { + spec.mesh = sphereMesh(cache); + spec.transform = localTransform*Matrix4::scaling(Vector3{Float(sphere->getRadius())}); + } else if(const auto* cylinder = dynamic_cast(&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(&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(&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 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 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 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(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 framePtrs; - for(const OpenSim::PhysicalFrame& frame: model.getComponentList()) { - if(frame.getProperty_attached_geometry().size() == 0) continue; + std::unordered_map 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()) { 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()) { + 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 markerPtrs; @@ -301,12 +444,25 @@ LoadedScene loadScene(const std::string& modelPath, scene.markers.push_back(std::move(track)); } + std::vector pathPtrs; + for(const OpenSim::GeometryPath& path: model.getComponentList()) { + 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(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; @@ -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, {}); - if(hasSceneBounds) scene.initialBounds = Math::join(scene.initialBounds, point); - else scene.initialBounds = point; - hasSceneBounds = true; + expandBoundsWithPoint(scene.initialBounds, hasSceneBounds, position); + } + + for(std::size_t i = 0; i != pathPtrs.size(); ++i) { + std::vector 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; diff --git a/src/OpenSimLoader.hpp b/src/OpenSimLoader.hpp index fe0149a..2ca9545 100644 --- a/src/OpenSimLoader.hpp +++ b/src/OpenSimLoader.hpp @@ -2,9 +2,11 @@ #include #include +#include #include #include +#include #include #include #include @@ -22,11 +24,17 @@ struct CpuMesh { Magnum::Range3D bounds; }; +enum class GeometryLayer { + Body, + Wrap, +}; + struct GeometrySpec { std::shared_ptr 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 positions; }; +struct PathTrack { + std::string name; + std::size_t maxPointCount{}; + std::vector colors; + std::vector> polylines; +}; + struct LoadedScene { std::string modelName; std::string motionName; std::vector times; std::vector frames; std::vector markers; + std::vector paths; Magnum::Range3D initialBounds; }; diff --git a/src/ViewerApp.cpp b/src/ViewerApp.cpp index f4da96f..539e504 100644 --- a/src/ViewerApp.cpp +++ b/src/ViewerApp.cpp @@ -15,7 +15,6 @@ #include #include -#include #include #include @@ -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 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 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 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 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 color, + std::shared_ptr 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::uploadMesh(const CpuMesh& mesh) c void ViewerApp::createSceneObjects() { std::unordered_map> 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(frame.object); - geometryObject->setTransformation(Matrix4::scaling(geometry.scale)); - auto drawable = std::make_unique(*geometryObject, _shader, *gpuMesh, geometry.color, _drawables); + geometryObject->setTransformation(geometry.transform); - frame.geometries.push_back({geometryObject.get(), geometry.color}); + auto color = std::make_shared(geometry.color); + auto drawable = std::make_unique( + *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(&_scene); markerObject->setTransformation(Matrix4::scaling(Vector3{MarkerRadius})); - auto drawable = std::make_unique(*markerObject, _shader, *_markerMesh, markerTrack.color, _drawables); - _markers.push_back({markerObject.get(), markerTrack.color}); + auto color = std::make_shared(markerTrack.color); + auto drawable = std::make_unique(*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(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(&_scene); + auto visible = std::make_shared(false); + auto drawable = std::make_unique(*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(&_scene); + auto visible = std::make_shared(false); + auto drawable = std::make_unique(*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(&_scene); object->setTransformation(Matrix4::translation(axis.translation)*Matrix4::scaling(axis.scale)); - auto drawable = std::make_unique(*object, _shader, *_boxMesh, axis.color, _drawables); + auto drawable = std::make_unique( + *object, + _shader, + *_boxMesh, + std::make_shared(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(&_scene); originObject->setTransformation(Matrix4::scaling(Vector3{originMarkerScale})); auto originDrawable = std::make_unique( - *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( - *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 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(); } diff --git a/src/ViewerApp.hpp b/src/ViewerApp.hpp index aa5efd4..45f51ff 100644 --- a/src/ViewerApp.hpp +++ b/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 visible; }; struct SceneFrame { Object3D* object{}; - std::vector geometries; }; struct SceneMarker { Object3D* object{}; - Magnum::Color4 color{1.0f}; + }; + + struct ScenePath { + std::shared_ptr color; + std::vector segments; + std::vector 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 color, + std::shared_ptr visible, DrawableGroup3D& drawables); private: @@ -98,7 +103,8 @@ private: Magnum::Shaders::PhongGL& _shader; GpuMesh& _mesh; - Magnum::Color4 _color; + std::shared_ptr _color; + std::shared_ptr _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> _objectStorage; std::vector _frames; std::vector _markers; + std::vector _paths; std::shared_ptr _markerMesh; + std::shared_ptr _cylinderMesh; std::shared_ptr _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 _lastPointerPosition;