diff --git a/src/BeamAdapter/component/engine/WireRestShape.inl b/src/BeamAdapter/component/engine/WireRestShape.inl index dcd08e25d..e6c9bd70c 100644 --- a/src/BeamAdapter/component/engine/WireRestShape.inl +++ b/src/BeamAdapter/component/engine/WireRestShape.inl @@ -173,8 +173,8 @@ bool WireRestShape::initTopology() l_topology->addEdge(i, i + 1); } - prev_length = length; - prev_edges = nbrVisuEdges; + prev_length += length; + prev_edges += nbrVisuEdges; startPtId = 1; // Assume the last point of mat[n] == first point of mat[n+1] } @@ -315,12 +315,23 @@ void WireRestShape::getRestTransformOnX(Transform &global_H_local, co } const type::vector& keyPts = d_keyPoints.getValue(); + // Walk all sections preceding the one that owns x_used and accumulate their tip transforms. + // Each section composes its local rest geometry with this predecessor frame, so chained non-straight + // sections (Spire after Spire, Mesh after Spire, ...) stay tangent-continuous instead of being placed + // along the world X axis at keyPts[i]. + Transform predecessor = Transform::identity(); for (sofa::Size i = 1; i < keyPts.size(); ++i) { if (x_used <= keyPts[i]) { - return l_sectionMaterials.get(i - 1)->getRestTransformOnX(global_H_local, x_used, keyPts[i - 1]); + return l_sectionMaterials.get(i - 1)->getRestTransformOnX( + global_H_local, x_used, keyPts[i - 1], predecessor); } + + Transform tip; + l_sectionMaterials.get(i - 1)->getRestTransformOnX( + tip, keyPts[i], keyPts[i - 1], predecessor); + predecessor = tip; } msg_error() << " problem in getRestTransformOnX : x_curv " << x_used << " is not between keyPoints" << d_keyPoints.getValue(); diff --git a/src/BeamAdapter/component/model/BaseRodSectionMaterial.h b/src/BeamAdapter/component/model/BaseRodSectionMaterial.h index cb74801dd..dff062679 100644 --- a/src/BeamAdapter/component/model/BaseRodSectionMaterial.h +++ b/src/BeamAdapter/component/model/BaseRodSectionMaterial.h @@ -93,12 +93,18 @@ class BaseRodSectionMaterial : public core::objectmodel::BaseComponent /// Returns the Young modulus, Poisson's and massDensity coefficient of this section void getMechanicalParameters(Real& youngModulus, Real& cPoisson, Real& massDensity) const; - /// This function is called to get the rest position of the beam depending on the current curved abscisse given in parameter - virtual void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) + /// This function is called to get the rest position of the beam depending on the current curved abscisse given in parameter. + /// @param predecessor_H_sectionStart locates this section's starting frame wrt world. Pass Transform::identity() + /// for the first section. Subsequent sections receive the accumulated tip transform of all preceding sections so + /// each section composes its local rest geometry with the actual end-frame of the predecessor instead of assuming + /// the wire so far ran straight along world +X. + virtual void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start, + const Transform& predecessor_H_sectionStart) { SOFA_UNUSED(global_H_local); SOFA_UNUSED(x_used); SOFA_UNUSED(x_start); + SOFA_UNUSED(predecessor_H_sectionStart); } protected: diff --git a/src/BeamAdapter/component/model/RodMeshSection.h b/src/BeamAdapter/component/model/RodMeshSection.h index da8183b9f..fed317a19 100644 --- a/src/BeamAdapter/component/model/RodMeshSection.h +++ b/src/BeamAdapter/component/model/RodMeshSection.h @@ -53,8 +53,11 @@ class RodMeshSection : public BaseRodSectionMaterial /// Default Constructor RodMeshSection(); - /// Override method to get the rest position of the beam. In this implementation, it will interpolate along the loaded mesh geometry - void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) override; + /// Override method to get the rest position of the beam. The mesh-interpolated rest transform is expressed in the + /// section's local frame (origin at the section start, +X = predecessor tip tangent) and composed with + /// @p predecessor_H_sectionStart to obtain the world rest transform. + void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start, + const Transform& predecessor_H_sectionStart) override; protected: /// Internal method to init the section. Called by @sa BaseRodSectionMaterial::init() method diff --git a/src/BeamAdapter/component/model/RodMeshSection.inl b/src/BeamAdapter/component/model/RodMeshSection.inl index 61eee6e4e..36e85ffe0 100644 --- a/src/BeamAdapter/component/model/RodMeshSection.inl +++ b/src/BeamAdapter/component/model/RodMeshSection.inl @@ -62,7 +62,8 @@ bool RodMeshSection::initSection() template -void RodMeshSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) +void RodMeshSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start, + const Transform& predecessor_H_sectionStart) { Real abs_curr = x_used - x_start; abs_curr = abs_curr /(this->d_length.getValue()) * m_absOfGeometry; @@ -97,11 +98,10 @@ void RodMeshSection::getRestTransformOnX(Transform& global_H_local, c p.getOrientation() = slerp; } - type::Vec3 PosEndCurve = p.getCenter(); - Quat ExtremityQuat = p.getOrientation(); - type::Vec3 ExtremityPos = PosEndCurve + type::Vec3(x_start, 0, 0); - - global_H_local.set(ExtremityPos, ExtremityQuat); + // p is expressed in the section-local frame (origin at section start, +X = predecessor tip tangent). + Transform sectionStart_H_local; + sectionStart_H_local.set(p.getCenter(), p.getOrientation()); + global_H_local = predecessor_H_sectionStart * sectionStart_H_local; } diff --git a/src/BeamAdapter/component/model/RodSpireSection.h b/src/BeamAdapter/component/model/RodSpireSection.h index 83a9c631e..3d9209ae9 100644 --- a/src/BeamAdapter/component/model/RodSpireSection.h +++ b/src/BeamAdapter/component/model/RodSpireSection.h @@ -50,8 +50,11 @@ class RodSpireSection : public BaseRodSectionMaterial /// Default Constructor RodSpireSection(); - /// Override method to get the rest position of the beam. In this implementation, it will compute the current position given the spire parameters - void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) override; + /// Override method to get the rest position of the beam. The spire is computed in the section's local frame + /// (origin at the section start, +X aligned with the predecessor tip tangent) and composed with + /// @p predecessor_H_sectionStart to obtain the world rest transform. + void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start, + const Transform& predecessor_H_sectionStart) override; protected: /// Internal method to init the section. Called by @sa BaseRodSectionMaterial::init() method diff --git a/src/BeamAdapter/component/model/RodSpireSection.inl b/src/BeamAdapter/component/model/RodSpireSection.inl index b15a4e2ad..f37c09cd0 100644 --- a/src/BeamAdapter/component/model/RodSpireSection.inl +++ b/src/BeamAdapter/component/model/RodSpireSection.inl @@ -53,7 +53,8 @@ bool RodSpireSection::initSection() template -void RodSpireSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) +void RodSpireSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start, + const Transform& predecessor_H_sectionStart) { Real projetedLength = d_spireDiameter.getValue() * M_PI; Real lengthSpire = sqrt(d_spireHeight.getValue() * d_spireHeight.getValue() + projetedLength * projetedLength); @@ -73,12 +74,13 @@ void RodSpireSection::getRestTransformOnX(Transform& global_H_local, Qtheta.axisToQuat(type::Vec3(0, 1, 0), theta); Quat newSpireQuat = Qtheta * Qphi; - // computation of the position + // computation of the position in the section-local frame (origin at section start, +X = predecessor tip tangent) Real radius = d_spireDiameter.getValue() / 2.0; type::Vec3 PosEndCurve(radius * sin(theta), numSpire * d_spireHeight.getValue(), radius * (cos(theta) - 1)); - type::Vec3 SpirePos = PosEndCurve + type::Vec3(x_start, 0, 0); - global_H_local.set(SpirePos, newSpireQuat); + Transform sectionStart_H_local; + sectionStart_H_local.set(PosEndCurve, newSpireQuat); + global_H_local = predecessor_H_sectionStart * sectionStart_H_local; } } // namespace beamadapter diff --git a/src/BeamAdapter/component/model/RodStraightSection.h b/src/BeamAdapter/component/model/RodStraightSection.h index e5fe157bb..3c46d36d9 100644 --- a/src/BeamAdapter/component/model/RodStraightSection.h +++ b/src/BeamAdapter/component/model/RodStraightSection.h @@ -47,8 +47,11 @@ class RodStraightSection : public BaseRodSectionMaterial /// Default Constructor RodStraightSection(); - /// Override method to get the rest position of the beam. In this implementation, it will basically returns Vec3(x_start + x_used, 0 0) - void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) override; + /// Override method to get the rest position of the beam. The local rest position along the section's own +X axis + /// is composed with @p predecessor_H_sectionStart so the section continues tangent to the predecessor's tip frame + /// rather than assuming a world-X anchored start. + void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start, + const Transform& predecessor_H_sectionStart) override; protected: /// Internal method to init the section. Called by @sa BaseRodSectionMaterial::init() method diff --git a/src/BeamAdapter/component/model/RodStraightSection.inl b/src/BeamAdapter/component/model/RodStraightSection.inl index e74851ca0..91b085cc4 100644 --- a/src/BeamAdapter/component/model/RodStraightSection.inl +++ b/src/BeamAdapter/component/model/RodStraightSection.inl @@ -51,9 +51,12 @@ bool RodStraightSection::initSection() template -void RodStraightSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) +void RodStraightSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start, + const Transform& predecessor_H_sectionStart) { - global_H_local.set(type::Vec3(x_start + x_used, 0.0, 0.0), Quat()); + Transform sectionStart_H_local; + sectionStart_H_local.set(type::Vec3(x_used - x_start, 0.0, 0.0), Quat()); + global_H_local = predecessor_H_sectionStart * sectionStart_H_local; }