diff --git a/src/OpenSimCreator/UI/PreviewExperimentalData/PreviewExperimentalDataTab.cpp b/src/OpenSimCreator/UI/PreviewExperimentalData/PreviewExperimentalDataTab.cpp index c4b7949e9..3021d5f11 100644 --- a/src/OpenSimCreator/UI/PreviewExperimentalData/PreviewExperimentalDataTab.cpp +++ b/src/OpenSimCreator/UI/PreviewExperimentalData/PreviewExperimentalDataTab.cpp @@ -216,7 +216,7 @@ namespace DataSeriesPattern::forDatatype("_fx", "_fy", "_fz"), // extra - DataSeriesPattern::forDatatype("_x", "_y", "_z"), + DataSeriesPattern::forDatatype("_x", "_y", "_z"), }; }; @@ -272,6 +272,21 @@ namespace std::vector m_Annotations; }; + + // Returns the elements associated with one datapoint (e.g. [x, y, z]) + std::vector extractDataPoint( + const SimTK::State& state, + const OpenSim::Storage& storage, + const DataSeriesAnnotation& annotation) + { + // lol, `OpenSim::Storage` API, etc. + int aN = annotation.firstColumnOffset + static_cast(numElementsIn(annotation.dataType)); + std::vector buffer(aN); + double* p = buffer.data(); + storage.getDataAtTime(state.getTime(), aN, &p); + buffer.erase(buffer.begin(), buffer.begin() + annotation.firstColumnOffset); + return buffer; + } } // ui-independent graphics helpers @@ -302,10 +317,21 @@ namespace template<> void generateDecorations( const SimTK::State&, - std::span, - SimTK::Array_&) + std::span data, + SimTK::Array_& out) { - // TODO + const SimTK::Vec3 position = { data[0], data[1], data[2] }; + if (not position.isNaN() and position.normSqr() > SimTK::Eps) { + SimTK::DecorativeArrow arrow{ + SimTK::Vec3(0.0), + position.normalize(), + }; + arrow.setScaleFactors({1, 1, 0.00001}); + arrow.setColor(to(Color::orange())); + arrow.setLineThickness(0.01); + arrow.setTipLength(0.1); + out.push_back(arrow); + } } template<> @@ -336,12 +362,24 @@ namespace } void generateDecorations( - const SimTK::State&, - const OpenSim::Storage&, - const DataSeriesAnnotation&, - SimTK::Array_&) + const SimTK::State& state, + const OpenSim::Storage& storage, + const DataSeriesAnnotation& annotation, + SimTK::Array_& out) { - // TODO + const auto data = extractDataPoint(state, storage, annotation); + OSC_ASSERT_ALWAYS(data.size() == numElementsIn(annotation.dataType)); + + static_assert(num_options() == 5); + switch (annotation.dataType) { + case DataPointType::Point: generateDecorations( state, std::span{data}, out); break; + case DataPointType::PointForce: generateDecorations( state, std::span{data}, out); break; + case DataPointType::BodyForce: generateDecorations( state, std::span{data}, out); break; + case DataPointType::Orientation: generateDecorations( state, std::span{data}, out); break; + + case DataPointType::Unknown: break; // do nothing + default: break; // do nothing + } } } diff --git a/src/oscar_simbody/SimTKDecorationGenerator.cpp b/src/oscar_simbody/SimTKDecorationGenerator.cpp index bfe93d8ba..9b70ff3ec 100644 --- a/src/oscar_simbody/SimTKDecorationGenerator.cpp +++ b/src/oscar_simbody/SimTKDecorationGenerator.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -66,7 +67,7 @@ namespace } // creates a geometry-to-ground transform for the given geometry - Transform ToOscTransform( + Transform ToOscTransformWithoutScaling( const SimTK::SimbodyMatterSubsystem& matter, const SimTK::State& state, const SimTK::DecorativeGeometry& g) @@ -75,7 +76,7 @@ namespace const SimTK::Transform& body2ground = mobod.getBodyTransform(state); const SimTK::Transform& decoration2body = g.getTransform(); - return to(body2ground * decoration2body).with_scale(GetScaleFactors(g)); + return to(body2ground * decoration2body); } size_t hash_of(const SimTK::Vec3& v) @@ -130,9 +131,14 @@ namespace } private: + Transform ToOscTransformWithoutScaling(const SimTK::DecorativeGeometry& d) const + { + return ::ToOscTransformWithoutScaling(m_Matter, m_State, d); + } + Transform ToOscTransform(const SimTK::DecorativeGeometry& d) const { - return ::ToOscTransform(m_Matter, m_State, d); + return ToOscTransformWithoutScaling(d).with_scale(GetScaleFactors(d)); } void implementPointGeometry(const SimTK::DecorativePoint&) final @@ -324,42 +330,17 @@ namespace void implementArrowGeometry(const SimTK::DecorativeArrow& d) final { - const Transform t = ToOscTransform(d); - - const Vec3 startBase = to(d.getStartPoint()); - const Vec3 endBase = to(d.getEndPoint()); - - const Vec3 start = transform_point(t, startBase); - const Vec3 end = transform_point(t, endBase); - - const Vec3 direction = normalize(end - start); - - const Vec3 neckStart = start; - const Vec3 neckEnd = end - (m_FixupScaleFactor * static_cast(d.getTipLength()) * direction); - const Vec3 headStart = neckEnd; - const Vec3 headEnd = end; - - const float neck_thickness = m_FixupScaleFactor * static_cast(d.getLineThickness()); - const float head_thickness = 1.75f * neck_thickness; - - const Color color = GetColor(d); - const auto flags = GetFlags(d); - - // emit neck cylinder - m_Consumer(SceneDecoration{ - .mesh = m_MeshCache.cylinder_mesh(), - .transform = cylinder_to_line_segment_transform({neckStart, neckEnd}, neck_thickness), - .shading = color, - .flags = flags, - }); - - // emit head cone - m_Consumer({ - .mesh = m_MeshCache.cone_mesh(), - .transform = cylinder_to_line_segment_transform({headStart, headEnd}, head_thickness), - .shading = color, - .flags = flags, - }); + const Transform t = ToOscTransformWithoutScaling(d); + const ArrowProperties p = { + .start = t * to(d.getStartPoint()), + .end = t * to(d.getEndPoint()), + .tip_length = static_cast(d.getTipLength()), + .neck_thickness = m_FixupScaleFactor * static_cast(d.getLineThickness()), + .head_thickness = 1.75f * m_FixupScaleFactor * static_cast(d.getLineThickness()), + .color = GetColor(d), + .decoration_flags = GetFlags(d), + }; + draw_arrow(m_MeshCache, p, m_Consumer); } void implementTorusGeometry(const SimTK::DecorativeTorus& d) final