diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 2f1e649..1886fb6 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -50,9 +50,9 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() "if this parameter is false, you draw the beam with " "color according to the force apply to each beam")), d_color(initData( - &d_color, - sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8), - "color", "The default beam color")), + &d_color, + sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8), + "color", "The default beam color")), d_index( initData(&d_index, "index", "if this parameter is false, you draw the beam with color " @@ -62,621 +62,621 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() "base of Cosserat models, 0 by default this can" "take another value if the rigid base is given " "by another body.")) { - this->addUpdateCallback( - "updateFrames", {&d_curv_abs_section, &d_curv_abs_frames, &d_debug}, - [this](const sofa::core::DataTracker &t) { + this->addUpdateCallback( + "updateFrames", {&d_curv_abs_section, &d_curv_abs_frames, &d_debug}, + [this](const sofa::core::DataTracker &t) { SOFA_UNUSED(t); this->initializeFrames(); const In1VecCoord &inDeform = - m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); + m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); this->updateExponentialSE3(inDeform); return sofa::core::objectmodel::ComponentState::Valid; - }, - {}); + }, + {}); } template void DiscreteCosseratMapping::doBaseCosseratInit() { - m_colorMap.setColorScheme("Blue to Red"); - m_colorMap.reinit(); + m_colorMap.setColorScheme("Blue to Red"); + m_colorMap.reinit(); } template void DiscreteCosseratMapping::apply( - const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutPos, - const vector &dataVecIn1Pos, - const vector &dataVecIn2Pos) { - - if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) - return; - - // Checking the componentState, to trigger a callback if other data fields (specifically - // d_curv_abs_section and d_curv_abs_frames) were changed dynamically - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; - /// Do Apply - // We need only one input In model and input Root model (if present) - const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); - const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); - - const auto sz = d_curv_abs_frames.getValue().size(); - OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); // frames states - out.resize(sz); - const auto baseIndex = d_baseIndex.getValue(); - - // update the Exponential matrices according to new deformation - // Here we update m_framesExponentialSE3Vectors & m_nodesExponentialSE3Vectors - // Which are the homogeneous matrices of the frames and the nodes in local - // coordinate. - this->updateExponentialSE3(in1); - - /* Apply the transformation to go from cossserat to SOFA frame*/ - Transform frame0 = - Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); - - // Cache the printLog value out of the loop, otherwise it will trigger a graph - // update at every iteration. - bool doPrintLog = this->f_printLog.getValue(); - for (unsigned int i = 0; i < sz; i++) { - Transform frame = frame0; - for (unsigned int u = 0; u < m_indicesVectors[i]; u++) { - frame *= - m_nodesExponentialSE3Vectors[u]; // frame = gX(L_0)*...*gX(L_{n-1}) + const sofa::core::MechanicalParams * /* mparams */, + const vector &dataVecOutPos, + const vector &dataVecIn1Pos, + const vector &dataVecIn2Pos) { + + if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) + return; + + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_curv_abs_section and d_curv_abs_frames) were changed dynamically + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + /// Do Apply + // We need only one input In model and input Root model (if present) + const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); + const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); + + const auto sz = d_curv_abs_frames.getValue().size(); + OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); // frames states + out.resize(sz); + const auto baseIndex = d_baseIndex.getValue(); + + // update the Exponential matrices according to new deformation + // Here we update m_framesExponentialSE3Vectors & m_nodesExponentialSE3Vectors + // Which are the homogeneous matrices of the frames and the nodes in local + // coordinate. + this->updateExponentialSE3(in1); + + /* Apply the transformation to go from cossserat to SOFA frame*/ + Transform frame0 = + Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); + + // Cache the printLog value out of the loop, otherwise it will trigger a graph + // update at every iteration. + bool doPrintLog = this->f_printLog.getValue(); + for (unsigned int i = 0; i < sz; i++) { + Transform frame = frame0; + for (unsigned int u = 0; u < m_indicesVectors[i]; u++) { + frame *= + m_nodesExponentialSE3Vectors[u]; // frame = gX(L_0)*...*gX(L_{n-1}) + } + frame *= m_framesExponentialSE3Vectors[i]; // frame*gX(x) + + // This is a lazy printing approach, so there is no time consuming action in + // the core of the loop. + msg_info_when(doPrintLog) << "Frame : " << i << " = " << frame; + + Vec3 origin = frame.getOrigin(); + Quat orientation = frame.getOrientation(); + out[i] = OutCoord(origin, orientation); } - frame *= m_framesExponentialSE3Vectors[i]; // frame*gX(x) - - // This is a lazy printing approach, so there is no time consuming action in - // the core of the loop. - msg_info_when(doPrintLog) << "Frame : " << i << " = " << frame; - - Vec3 origin = frame.getOrigin(); - Quat orientation = frame.getOrientation(); - out[i] = OutCoord(origin, orientation); - } - - // If the printLog attribute is checked then print distance between out - // frames. - if (doPrintLog) { - std::stringstream tmp; - for (unsigned int i = 0; i < out.size() - 1; i++) { - Vec3 diff = out[i + 1].getCenter() - out[i].getCenter(); - tmp << "dist " << i << " : " << diff.norm() << msgendl; + + // If the printLog attribute is checked then print distance between out + // frames. + if (doPrintLog) { + std::stringstream tmp; + for (unsigned int i = 0; i < out.size() - 1; i++) { + Vec3 diff = out[i + 1].getCenter() - out[i].getCenter(); + tmp << "dist " << i << " : " << diff.norm() << msgendl; + } + msg_info() << tmp.str(); } - msg_info() << tmp.str(); - } - // TODO(dmarchal:2024/06/13): This looks a suspicious design pattern, - // elaborate more on the purpose of m_indexInput and how to use it. - m_indexInput = 0; + // TODO(dmarchal:2024/06/13): This looks a suspicious design pattern, + // elaborate more on the purpose of m_indexInput and how to use it. + m_indexInput = 0; } template void DiscreteCosseratMapping::computeLogarithm( - const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { - - // computes theta - double theta = computeTheta(x, gX); - - // if theta is very small we retun log_gX as the identity. - if (theta <= std::numeric_limits::epsilon()) { - log_gX = Mat4x4::Identity(); - return; - } - - // otherwise we compute it - double csc_theta = 1.0 / (sin(x * theta / 2.0)); - double sec_theta = 1.0 / (cos(x * theta / 2.0)); - double cst = (1.0 / 8) * (csc_theta * csc_theta * csc_theta) * sec_theta; - double x_theta = x * theta; - double cos_2Xtheta = cos(2.0 * x_theta); - double cos_Xtheta = cos(x_theta); - double sin_2Xtheta = sin(2.0 * x_theta); - double sin_Xtheta = sin(x_theta); - - log_gX.clear(); - log_gX = cst * ((x_theta * cos_2Xtheta - sin_Xtheta) * Mat4x4::Identity() - - (x_theta * cos_Xtheta + 2.0 * x_theta * cos_2Xtheta - - sin_Xtheta - sin_2Xtheta) * - gX + - (2.0 * x_theta * cos_Xtheta + x_theta * cos_2Xtheta - - sin_Xtheta - sin_2Xtheta) * - (gX * gX) - - (x_theta * cos_Xtheta - sin_Xtheta) * (gX * gX * gX)); + const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { + + // computes theta + double theta = computeTheta(x, gX); + + // if theta is very small we retun log_gX as the identity. + if (theta <= std::numeric_limits::epsilon()) { + log_gX = Mat4x4::Identity(); + return; + } + + // otherwise we compute it + double csc_theta = 1.0 / (sin(x * theta / 2.0)); + double sec_theta = 1.0 / (cos(x * theta / 2.0)); + double cst = (1.0 / 8) * (csc_theta * csc_theta * csc_theta) * sec_theta; + double x_theta = x * theta; + double cos_2Xtheta = cos(2.0 * x_theta); + double cos_Xtheta = cos(x_theta); + double sin_2Xtheta = sin(2.0 * x_theta); + double sin_Xtheta = sin(x_theta); + + log_gX.clear(); + log_gX = cst * ((x_theta * cos_2Xtheta - sin_Xtheta) * Mat4x4::Identity() - + (x_theta * cos_Xtheta + 2.0 * x_theta * cos_2Xtheta - + sin_Xtheta - sin_2Xtheta) * + gX + + (2.0 * x_theta * cos_Xtheta + x_theta * cos_2Xtheta - + sin_Xtheta - sin_2Xtheta) * + (gX * gX) - + (x_theta * cos_Xtheta - sin_Xtheta) * (gX * gX * gX)); } template void DiscreteCosseratMapping::applyJ( - const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutVel, - const vector &dataVecIn1Vel, - const vector &dataVecIn2Vel) { - - if (dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty()) - return; - - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; - if (d_debug.getValue()) - std::cout << " ########## ApplyJ Function ########" << std::endl; - const In1VecDeriv &in1_vel = dataVecIn1Vel[0]->getValue(); - const In2VecDeriv &in2_vel = dataVecIn2Vel[0]->getValue(); - OutVecDeriv &out_vel = *dataVecOutVel[0]->beginEdit(); - const auto baseIndex = d_baseIndex.getValue(); - - // Curv abscissa of nodes and frames - sofa::helper::ReadAccessor>> curv_abs_section = - d_curv_abs_section; - sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; - - const In1VecCoord &inDeform = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()) - ->getValue(); // Strains - // Compute the tangent Exponential SE3 vectors - this->updateTangExpSE3(inDeform); - - // Get base velocity as input this is also called eta - m_nodesVelocityVectors.clear(); - - // Get base velocity and convert to Vec6, for the facility of computation - Vec6 baseVelocity; // - for (auto u = 0; u < 6; u++) - baseVelocity[u] = in2_vel[baseIndex][u]; - - // Apply the local transform i.e. from SOFA's frame to Cosserat's frame - const In2VecCoord &xfrom2Data = - m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); - Transform TInverse = Transform(xfrom2Data[baseIndex].getCenter(), - xfrom2Data[baseIndex].getOrientation()) - .inversed(); - Mat6x6 P = this->buildProjector(TInverse); - Vec6 baseLocalVelocity = - P * baseVelocity; // This is the base velocity in Locale frame - m_nodesVelocityVectors.push_back(baseLocalVelocity); - if (d_debug.getValue()) - std::cout << "Base local Velocity :" << baseLocalVelocity << std::endl; - - // Compute velocity at nodes - for (unsigned int i = 1; i < curv_abs_section.size(); i++) { - Transform Trans = m_nodesExponentialSE3Vectors[i].inversed(); - TangentTransform Adjoint; - Adjoint.clear(); - this->computeAdjoint(Trans, Adjoint); - - /// The null vector is replace by the linear velocity in Vec6Type - Vec6 Xi_dot = Vec6(in1_vel[i - 1], Vec3(0.0, 0.0, 0.0)); - - Vec6 eta_node_i = Adjoint * (m_nodesVelocityVectors[i - 1] + - m_nodesTangExpVectors[i] * Xi_dot); - m_nodesVelocityVectors.push_back(eta_node_i); + const sofa::core::MechanicalParams * /* mparams */, + const vector &dataVecOutVel, + const vector &dataVecIn1Vel, + const vector &dataVecIn2Vel) { + + if (dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty()) + return; + + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + if (d_debug.getValue()) + std::cout << " ########## ApplyJ Function ########" << std::endl; + const In1VecDeriv &in1_vel = dataVecIn1Vel[0]->getValue(); + const In2VecDeriv &in2_vel = dataVecIn2Vel[0]->getValue(); + OutVecDeriv &out_vel = *dataVecOutVel[0]->beginEdit(); + const auto baseIndex = d_baseIndex.getValue(); + + // Curv abscissa of nodes and frames + sofa::helper::ReadAccessor>> curv_abs_section = + d_curv_abs_section; + sofa::helper::ReadAccessor>> curv_abs_frames = + d_curv_abs_frames; + + const In1VecCoord &inDeform = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()) + ->getValue(); // Strains + // Compute the tangent Exponential SE3 vectors + this->updateTangExpSE3(inDeform); + + // Get base velocity as input this is also called eta + m_nodesVelocityVectors.clear(); + + // Get base velocity and convert to Vec6, for the facility of computation + Vec6 baseVelocity; // + for (auto u = 0; u < 6; u++) + baseVelocity[u] = in2_vel[baseIndex][u]; + + // Apply the local transform i.e. from SOFA's frame to Cosserat's frame + const In2VecCoord &xfrom2Data = + m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); + Transform TInverse = Transform(xfrom2Data[baseIndex].getCenter(), + xfrom2Data[baseIndex].getOrientation()) + .inversed(); + Mat6x6 P = this->buildProjector(TInverse); + Vec6 baseLocalVelocity = + P * baseVelocity; // This is the base velocity in Locale frame + m_nodesVelocityVectors.push_back(baseLocalVelocity); if (d_debug.getValue()) - std::cout << "Node velocity : " << i << " = " << eta_node_i << std::endl; - } - - const OutVecCoord &out = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - auto sz = curv_abs_frames.size(); - out_vel.resize(sz); - for (unsigned int i = 0; i < sz; i++) { - Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); - TangentTransform - Adjoint; ///< the class insure that the constructed adjoint is zeroed. - Adjoint.clear(); - this->computeAdjoint(Trans, Adjoint); - Vec6 frame_Xi_dot; - - for (auto u = 0; u < 3; u++) { - frame_Xi_dot(u) = in1_vel[m_indicesVectors[i] - 1][u]; - frame_Xi_dot(u + 3) = 0.; + std::cout << "Base local Velocity :" << baseLocalVelocity << std::endl; + + // Compute velocity at nodes + for (unsigned int i = 1; i < curv_abs_section.size(); i++) { + Transform Trans = m_nodesExponentialSE3Vectors[i].inversed(); + TangentTransform Adjoint; + Adjoint.clear(); + this->computeAdjoint(Trans, Adjoint); + + /// The null vector is replace by the linear velocity in Vec6Type + Vec6 Xi_dot = Vec6(in1_vel[i - 1], Vec3(0.0, 0.0, 0.0)); + + Vec6 eta_node_i = Adjoint * (m_nodesVelocityVectors[i - 1] + + m_nodesTangExpVectors[i] * Xi_dot); + m_nodesVelocityVectors.push_back(eta_node_i); + if (d_debug.getValue()) + std::cout << "Node velocity : " << i << " = " << eta_node_i << std::endl; } - Vec6 eta_frame_i = - Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i] - 1] + - m_framesTangExpVectors[i] * frame_Xi_dot); // eta - auto T = Transform(out[i].getCenter(), out[i].getOrientation()); - Mat6x6 Proj = this->buildProjector(T); + const OutVecCoord &out = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + auto sz = curv_abs_frames.size(); + out_vel.resize(sz); + for (unsigned int i = 0; i < sz; i++) { + Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); + TangentTransform + Adjoint; ///< the class insure that the constructed adjoint is zeroed. + Adjoint.clear(); + this->computeAdjoint(Trans, Adjoint); + Vec6 frame_Xi_dot; + + for (auto u = 0; u < 3; u++) { + frame_Xi_dot(u) = in1_vel[m_indicesVectors[i] - 1][u]; + frame_Xi_dot(u + 3) = 0.; + } + Vec6 eta_frame_i = + Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i] - 1] + + m_framesTangExpVectors[i] * frame_Xi_dot); // eta - out_vel[i] = Proj * eta_frame_i; + auto T = Transform(out[i].getCenter(), out[i].getOrientation()); + Mat6x6 Proj = this->buildProjector(T); - if (d_debug.getValue()) - std::cout << "Frame velocity : " << i << " = " << eta_frame_i - << std::endl; - } - dataVecOutVel[0]->endEdit(); - m_indexInput = 0; + out_vel[i] = Proj * eta_frame_i; + + if (d_debug.getValue()) + std::cout << "Frame velocity : " << i << " = " << eta_frame_i + << std::endl; + } + dataVecOutVel[0]->endEdit(); + m_indexInput = 0; } template void DiscreteCosseratMapping::applyJT( - const sofa::core::MechanicalParams * /*mparams*/, - const vector &dataVecOut1Force, - const vector &dataVecOut2Force, - const vector &dataVecInForce) { - - if (dataVecOut1Force.empty() || dataVecInForce.empty() || - dataVecOut2Force.empty()) - return; - - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; - - if (d_debug.getValue()) - std::cout << " ########## ApplyJT force Function ########" << std::endl; - const OutVecDeriv &in = dataVecInForce[0]->getValue(); - - In1VecDeriv &out1 = *dataVecOut1Force[0]->beginEdit(); - In2VecDeriv &out2 = *dataVecOut2Force[0]->beginEdit(); - const auto baseIndex = d_baseIndex.getValue(); - - const OutVecCoord &frame = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord x1from = x1fromData->getValue(); - vector local_F_Vec; - local_F_Vec.clear(); - - out1.resize(x1from.size()); - - // convert the input from Deriv type to vec6 type, for the purpose of the - // matrix vector multiplication - for (unsigned int var = 0; var < in.size(); ++var) { - Vec6 vec; - for (unsigned j = 0; j < 6; j++) - vec[j] = in[var][j]; - // Convert input from global frame(SOFA) to local frame - Transform _T = - Transform(frame[var].getCenter(), frame[var].getOrientation()); - Mat6x6 P_trans = (this->buildProjector(_T)); - P_trans.transpose(); - Vec6 local_F = P_trans * vec; - local_F_Vec.push_back(local_F); - } - - // Compute output forces - auto sz = m_indicesVectors.size(); - auto index = m_indicesVectors[sz - 1]; - m_totalBeamForceVectors.clear(); - m_totalBeamForceVectors.resize(sz); - - Vec6 F_tot; - F_tot.clear(); - m_totalBeamForceVectors.push_back(F_tot); - - Mat3x6 matB_trans; - matB_trans.clear(); - for (unsigned int k = 0; k < 3; k++) - matB_trans[k][k] = 1.0; - - for (auto s = sz; s--;) { - Mat6x6 coAdjoint; - - this->computeCoAdjoint( - m_framesExponentialSE3Vectors[s], - coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply - Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; - Mat6x6 temp = - m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in - // applyJ (here we transpose) - temp.transpose(); - Vec3 f = matB_trans * temp * node_F_Vec; - - if (index != m_indicesVectors[s]) { - index--; - // bring F_tot to the reference of the new beam - this->computeCoAdjoint( - m_nodesExponentialSE3Vectors[index], - coAdjoint); // m_nodesExponentialSE3Vectors computed in apply - F_tot = coAdjoint * F_tot; - Mat6x6 temp = m_nodesTangExpVectors[index]; - temp.transpose(); - // apply F_tot to the new beam - Vec3 temp_f = matB_trans * temp * F_tot; - out1[index - 1] += temp_f; - } - if (d_debug.getValue()) - std::cout << "f at s =" << s << " and index" << index << " is : " << f - << std::endl; + const sofa::core::MechanicalParams * /*mparams*/, + const vector &dataVecOut1Force, + const vector &dataVecOut2Force, + const vector &dataVecInForce) { - // compute F_tot - F_tot += node_F_Vec; - out1[m_indicesVectors[s] - 1] += f; - } + if (dataVecOut1Force.empty() || dataVecInForce.empty() || + dataVecOut2Force.empty()) + return; - Transform frame0 = Transform(frame[0].getCenter(), frame[0].getOrientation()); - Mat6x6 M = this->buildProjector(frame0); - out2[baseIndex] += M * F_tot; + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; - if (d_debug.getValue()) { - std::cout << "Node forces " << out1 << std::endl; - std::cout << "base Force: " << out2[baseIndex] << std::endl; - } + if (d_debug.getValue()) + std::cout << " ########## ApplyJT force Function ########" << std::endl; + const OutVecDeriv &in = dataVecInForce[0]->getValue(); + + In1VecDeriv &out1 = *dataVecOut1Force[0]->beginEdit(); + In2VecDeriv &out2 = *dataVecOut2Force[0]->beginEdit(); + const auto baseIndex = d_baseIndex.getValue(); + + const OutVecCoord &frame = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In1DataVecCoord *x1fromData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord x1from = x1fromData->getValue(); + vector local_F_Vec; + local_F_Vec.clear(); + + out1.resize(x1from.size()); + + // convert the input from Deriv type to vec6 type, for the purpose of the + // matrix vector multiplication + for (unsigned int var = 0; var < in.size(); ++var) { + Vec6 vec; + for (unsigned j = 0; j < 6; j++) + vec[j] = in[var][j]; + // Convert input from global frame(SOFA) to local frame + Transform _T = + Transform(frame[var].getCenter(), frame[var].getOrientation()); + Mat6x6 P_trans = (this->buildProjector(_T)); + P_trans.transpose(); + Vec6 local_F = P_trans * vec; + local_F_Vec.push_back(local_F); + } - dataVecOut1Force[0]->endEdit(); - dataVecOut2Force[0]->endEdit(); -} + // Compute output forces + auto sz = m_indicesVectors.size(); + auto index = m_indicesVectors[sz - 1]; + m_totalBeamForceVectors.clear(); + m_totalBeamForceVectors.resize(sz); -template -void DiscreteCosseratMapping::applyJT( - const sofa::core::ConstraintParams * /*cparams*/, - const vector &dataMatOut1Const, - const vector &dataMatOut2Const, - const vector &dataMatInConst) { - if (dataMatOut1Const.empty() || dataMatOut2Const.empty() || - dataMatInConst.empty()) - return; - - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; - - if (d_debug.getValue()) - std::cout << " ########## ApplyJT Constraint Function ########" - << std::endl; - // We need only one input In model and input Root model (if present) - In1MatrixDeriv &out1 = - *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space - // (reduced coordinate) - In2MatrixDeriv &out2 = - *dataMatOut2Const[0] - ->beginEdit(); // constraints on the reference frame (base frame) - const OutMatrixDeriv &in = - dataMatInConst[0] - ->getValue(); // input constraints defined on the mapped frames - - const OutVecCoord &frame = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord x1from = x1fromData->getValue(); - - Mat3x6 matB_trans; - matB_trans.clear(); - for (unsigned int k = 0; k < 3; k++) - matB_trans[k][k] = 1.0; - - vector> NodesInvolved; - vector> NodesInvolvedCompressed; - // helper::vector NodesConstraintDirection; - - typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); - - for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); - rowIt != rowItEnd; ++rowIt) { - if (d_debug.getValue()) { - std::cout << "************* Apply JT (MatrixDeriv) iteration on line "; - std::cout << rowIt.index(); - std::cout << "************* " << std::endl; - } - typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); - - // Creates a constraints if the input constraint is not empty. - if (colIt == colItEnd) { - if (d_debug.getValue()) { - std::cout << "no column for this constraint" << std::endl; - } - continue; - } - typename In1MatrixDeriv::RowIterator o1 = - out1.writeLine(rowIt.index()); // we store the constraint number - typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); - - NodesInvolved.clear(); - while (colIt != colItEnd) { - int childIndex = colIt.index(); - - const OutDeriv valueConst_ = colIt.val(); - Vec6 valueConst; - for (unsigned j = 0; j < 6; j++) - valueConst[j] = valueConst_[j]; - - int indexBeam = m_indicesVectors[childIndex]; - - Transform _T = Transform(frame[childIndex].getCenter(), - frame[childIndex].getOrientation()); - Mat6x6 P_trans = (this->buildProjector(_T)); - P_trans.transpose(); - - Mat6x6 coAdjoint; - this->computeCoAdjoint( - m_framesExponentialSE3Vectors[childIndex], - coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply - Mat6x6 temp = - m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] - // computed in applyJ - // (here we transpose) - temp.transpose(); - - Vec6 local_F = - coAdjoint * P_trans * - valueConst; // constraint direction in local frame of the beam. - - Vec3 f = matB_trans * temp * - local_F; // constraint direction in the strain space. - - o1.addCol(indexBeam - 1, f); - std::tuple test = std::make_tuple(indexBeam, local_F); - - NodesInvolved.push_back(test); - colIt++; - } - if (d_debug.getValue()) { - std::cout << "==> NodesInvolved : " << std::endl; - for (size_t i = 0; i < NodesInvolved.size(); i++) - std::cout << "index :" << get<0>(NodesInvolved[i]) - << " force :" << get<1>(NodesInvolved[i]) << "\n "; - } + Vec6 F_tot; + F_tot.clear(); + m_totalBeamForceVectors.push_back(F_tot); - // sort the Nodes Invoved by decreasing order - std::sort( - begin(NodesInvolved), end(NodesInvolved), - [](std::tuple const &t1, std::tuple const &t2) { - return std::get<0>(t1) > std::get<0>(t2); // custom compare function - }); + Mat3x6 matB_trans; + matB_trans.clear(); + for (unsigned int k = 0; k < 3; k++) + matB_trans[k][k] = 1.0; - NodesInvolvedCompressed.clear(); - - for (unsigned n = 0; n < NodesInvolved.size(); n++) { - std::tuple test_i = NodesInvolved[n]; - int numNode_i = std::get<0>(test_i); - Vec6 cumulativeF = std::get<1>(test_i); - - if (n < NodesInvolved.size() - 1) { - std::tuple test_i1 = NodesInvolved[n + 1]; - int numNode_i1 = std::get<0>(test_i1); - - while (numNode_i == numNode_i1) { - cumulativeF += std::get<1>(test_i1); - //// This was if ((n!=NodesInvolved.size()-2)||(n==0)) before and I - /// change it to - /// if ((n!=NodesInvolved.size()-1)||(n==0)) since the code can't - /// leave the will loop - if ((n != NodesInvolved.size() - 1) || (n == 0)) { - n++; - break; - } - test_i1 = NodesInvolved[n + 1]; - numNode_i1 = std::get<0>(test_i1); + for (auto s = sz; s--;) { + Mat6x6 coAdjoint; + + this->computeCoAdjoint( + m_framesExponentialSE3Vectors[s], + coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply + Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; + Mat6x6 temp = + m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in + // applyJ (here we transpose) + temp.transpose(); + Vec3 f = matB_trans * temp * node_F_Vec; + + if (index != m_indicesVectors[s]) { + index--; + // bring F_tot to the reference of the new beam + this->computeCoAdjoint( + m_nodesExponentialSE3Vectors[index], + coAdjoint); // m_nodesExponentialSE3Vectors computed in apply + F_tot = coAdjoint * F_tot; + Mat6x6 temp = m_nodesTangExpVectors[index]; + temp.transpose(); + // apply F_tot to the new beam + Vec3 temp_f = matB_trans * temp * F_tot; + out1[index - 1] += temp_f; } - } - NodesInvolvedCompressed.push_back( - std::make_tuple(numNode_i, cumulativeF)); + if (d_debug.getValue()) + std::cout << "f at s =" << s << " and index" << index << " is : " << f + << std::endl; + + // compute F_tot + F_tot += node_F_Vec; + out1[m_indicesVectors[s] - 1] += f; } + Transform frame0 = Transform(frame[0].getCenter(), frame[0].getOrientation()); + Mat6x6 M = this->buildProjector(frame0); + out2[baseIndex] += M * F_tot; + if (d_debug.getValue()) { - std::cout << " NodesInvolved after sort and compress" << std::endl; - for (size_t i = 0; i < NodesInvolvedCompressed.size(); i++) - std::cout << "index :" << get<0>(NodesInvolvedCompressed[i]) - << " force :" << get<1>(NodesInvolvedCompressed[i]) << "\n "; + std::cout << "Node forces " << out1 << std::endl; + std::cout << "base Force: " << out2[baseIndex] << std::endl; } - for (unsigned n = 0; n < NodesInvolvedCompressed.size(); n++) { - std::tuple test = NodesInvolvedCompressed[n]; - int numNode = std::get<0>(test); - int i = numNode; - Vec6 CumulativeF = std::get<1>(test); + dataVecOut1Force[0]->endEdit(); + dataVecOut2Force[0]->endEdit(); +} - while (i > 0) { - // cumulate on beam frame - Mat6x6 coAdjoint; - this->computeCoAdjoint( - m_nodesExponentialSE3Vectors[i - 1], - coAdjoint); // m_nodesExponentialSE3Vectors computed in apply - CumulativeF = coAdjoint * CumulativeF; - // transfer to strain space (local coordinates) - Mat6x6 temp = m_nodesTangExpVectors[i - 1]; - temp.transpose(); - Vec3 temp_f = matB_trans * temp * CumulativeF; +template +void DiscreteCosseratMapping::applyJT( + const sofa::core::ConstraintParams * /*cparams*/, + const vector &dataMatOut1Const, + const vector &dataMatOut2Const, + const vector &dataMatInConst) { + if (dataMatOut1Const.empty() || dataMatOut2Const.empty() || + dataMatInConst.empty()) + return; - if (i > 1) - o1.addCol(i - 2, temp_f); - i--; - } + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; - Transform frame0 = - Transform(frame[0].getCenter(), frame[0].getOrientation()); - Mat6x6 M = this->buildProjector(frame0); + if (d_debug.getValue()) + std::cout << " ########## ApplyJT Constraint Function ########" + << std::endl; + // We need only one input In model and input Root model (if present) + In1MatrixDeriv &out1 = + *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space + // (reduced coordinate) + In2MatrixDeriv &out2 = + *dataMatOut2Const[0] + ->beginEdit(); // constraints on the reference frame (base frame) + const OutMatrixDeriv &in = + dataMatInConst[0] + ->getValue(); // input constraints defined on the mapped frames + + const OutVecCoord &frame = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In1DataVecCoord *x1fromData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord x1from = x1fromData->getValue(); + + Mat3x6 matB_trans; + matB_trans.clear(); + for (unsigned int k = 0; k < 3; k++) + matB_trans[k][k] = 1.0; + + vector> NodesInvolved; + vector> NodesInvolvedCompressed; + // helper::vector NodesConstraintDirection; + + typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + + for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); + rowIt != rowItEnd; ++rowIt) { + if (d_debug.getValue()) { + std::cout << "************* Apply JT (MatrixDeriv) iteration on line "; + std::cout << rowIt.index(); + std::cout << "************* " << std::endl; + } + typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); + typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); + + // Creates a constraints if the input constraint is not empty. + if (colIt == colItEnd) { + if (d_debug.getValue()) { + std::cout << "no column for this constraint" << std::endl; + } + continue; + } + typename In1MatrixDeriv::RowIterator o1 = + out1.writeLine(rowIt.index()); // we store the constraint number + typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); + + NodesInvolved.clear(); + while (colIt != colItEnd) { + int childIndex = colIt.index(); + + const OutDeriv valueConst_ = colIt.val(); + Vec6 valueConst; + for (unsigned j = 0; j < 6; j++) + valueConst[j] = valueConst_[j]; + + int indexBeam = m_indicesVectors[childIndex]; + + Transform _T = Transform(frame[childIndex].getCenter(), + frame[childIndex].getOrientation()); + Mat6x6 P_trans = (this->buildProjector(_T)); + P_trans.transpose(); + + Mat6x6 coAdjoint; + this->computeCoAdjoint( + m_framesExponentialSE3Vectors[childIndex], + coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply + Mat6x6 temp = + m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] + // computed in applyJ + // (here we transpose) + temp.transpose(); + + Vec6 local_F = + coAdjoint * P_trans * + valueConst; // constraint direction in local frame of the beam. + + Vec3 f = matB_trans * temp * + local_F; // constraint direction in the strain space. + + o1.addCol(indexBeam - 1, f); + std::tuple test = std::make_tuple(indexBeam, local_F); + + NodesInvolved.push_back(test); + colIt++; + } + if (d_debug.getValue()) { + std::cout << "==> NodesInvolved : " << std::endl; + for (size_t i = 0; i < NodesInvolved.size(); i++) + std::cout << "index :" << get<0>(NodesInvolved[i]) + << " force :" << get<1>(NodesInvolved[i]) << "\n "; + } + + // sort the Nodes Invoved by decreasing order + std::sort( + begin(NodesInvolved), end(NodesInvolved), + [](std::tuple const &t1, std::tuple const &t2) { + return std::get<0>(t1) > std::get<0>(t2); // custom compare function + }); + + NodesInvolvedCompressed.clear(); + + for (unsigned n = 0; n < NodesInvolved.size(); n++) { + std::tuple test_i = NodesInvolved[n]; + int numNode_i = std::get<0>(test_i); + Vec6 cumulativeF = std::get<1>(test_i); + + if (n < NodesInvolved.size() - 1) { + std::tuple test_i1 = NodesInvolved[n + 1]; + int numNode_i1 = std::get<0>(test_i1); + + while (numNode_i == numNode_i1) { + cumulativeF += std::get<1>(test_i1); + //// This was if ((n!=NodesInvolved.size()-2)||(n==0)) before and I + /// change it to + /// if ((n!=NodesInvolved.size()-1)||(n==0)) since the code can't + /// leave the will loop + if ((n != NodesInvolved.size() - 1) || (n == 0)) { + n++; + break; + } + test_i1 = NodesInvolved[n + 1]; + numNode_i1 = std::get<0>(test_i1); + } + } + NodesInvolvedCompressed.push_back( + std::make_tuple(numNode_i, cumulativeF)); + } + + if (d_debug.getValue()) { + std::cout << " NodesInvolved after sort and compress" << std::endl; + for (size_t i = 0; i < NodesInvolvedCompressed.size(); i++) + std::cout << "index :" << get<0>(NodesInvolvedCompressed[i]) + << " force :" << get<1>(NodesInvolvedCompressed[i]) << "\n "; + } - Vec6 base_force = M * CumulativeF; - o2.addCol(d_baseIndex.getValue(), base_force); + for (unsigned n = 0; n < NodesInvolvedCompressed.size(); n++) { + std::tuple test = NodesInvolvedCompressed[n]; + int numNode = std::get<0>(test); + int i = numNode; + Vec6 CumulativeF = std::get<1>(test); + + while (i > 0) { + // cumulate on beam frame + Mat6x6 coAdjoint; + this->computeCoAdjoint( + m_nodesExponentialSE3Vectors[i - 1], + coAdjoint); // m_nodesExponentialSE3Vectors computed in apply + CumulativeF = coAdjoint * CumulativeF; + // transfer to strain space (local coordinates) + Mat6x6 temp = m_nodesTangExpVectors[i - 1]; + temp.transpose(); + Vec3 temp_f = matB_trans * temp * CumulativeF; + + if (i > 1) + o1.addCol(i - 2, temp_f); + i--; + } + + Transform frame0 = + Transform(frame[0].getCenter(), frame[0].getOrientation()); + Mat6x6 M = this->buildProjector(frame0); + + Vec6 base_force = M * CumulativeF; + o2.addCol(d_baseIndex.getValue(), base_force); + } } - } - //"""END ARTICULATION SYSTEM MAPPING""" - dataMatOut1Const[0]->endEdit(); - dataMatOut2Const[0]->endEdit(); + //"""END ARTICULATION SYSTEM MAPPING""" + dataMatOut1Const[0]->endEdit(); + dataMatOut2Const[0]->endEdit(); } template void DiscreteCosseratMapping::computeBBox( - const sofa::core::ExecParams *, bool) { - const OutVecCoord &x = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - - SReal minBBox[3] = {std::numeric_limits::max(), - std::numeric_limits::max(), - std::numeric_limits::max()}; - SReal maxBBox[3] = {-std::numeric_limits::max(), - -std::numeric_limits::max(), - -std::numeric_limits::max()}; - for (std::size_t i = 0; i < x.size(); i++) { - const OutCoord &p = x[i]; - for (int c = 0; c < 3; c++) { - if (p[c] > maxBBox[c]) - maxBBox[c] = p[c]; - if (p[c] < minBBox[c]) - minBBox[c] = p[c]; + const sofa::core::ExecParams *, bool) { + const OutVecCoord &x = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + + SReal minBBox[3] = {std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()}; + SReal maxBBox[3] = {-std::numeric_limits::max(), + -std::numeric_limits::max(), + -std::numeric_limits::max()}; + for (std::size_t i = 0; i < x.size(); i++) { + const OutCoord &p = x[i]; + for (int c = 0; c < 3; c++) { + if (p[c] > maxBBox[c]) + maxBBox[c] = p[c]; + if (p[c] < minBBox[c]) + minBBox[c] = p[c]; + } } - } - this->f_bbox.setValue(sofa::type::TBoundingBox(minBBox, maxBBox)); + this->f_bbox.setValue(sofa::type::TBoundingBox(minBBox, maxBBox)); } template -void DiscreteCosseratMapping::draw( - const sofa::core::visual::VisualParams *vparams) { - if (!vparams->displayFlags().getShowMechanicalMappings()) - return; - - // draw cable - typedef RGBAColor RGBAColor; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - const OutDataVecCoord *xfromData = - m_toModel->read(sofa::core::ConstVecCoordId::position()); - const OutVecCoord xData = xfromData->getValue(); - vector positions; - vector> Orientation; - positions.clear(); - Orientation.clear(); - unsigned int sz = xData.size(); - for (unsigned int i = 0; i < sz; i++) { - positions.push_back(xData[i].getCenter()); - Orientation.push_back(xData[i].getOrientation()); - } - - // Get access articulated - const In1DataVecCoord *artiData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord xPos = artiData->getValue(); - - RGBAColor drawColor = d_color.getValue(); - // draw each segment of the beam as a cylinder. - for (unsigned int i = 0; i < sz - 1; i++) - vparams->drawTool()->drawCylinder(positions[i], positions[i + 1], - d_radius.getValue(), drawColor); - - // Define color map - SReal min = d_min.getValue(); - SReal max = d_max.getValue(); - sofa::helper::ColorMap::evaluator _eval = - m_colorMap.getEvaluator(min, max); - - glLineWidth(d_radius.getValue()); - glBegin(GL_LINES); - if (d_drawMapBeam.getValue()) { - sofa::type::RGBAColor _color = d_color.getValue(); - RGBAColor colorL = RGBAColor(_color[0], _color[1], _color[2], _color[3]); - glColor4f(colorL[0], colorL[1], colorL[2], colorL[3]); - for (unsigned int i = 0; i < sz - 1; i++) { - vparams->drawTool()->drawLine(positions[i], positions[i + 1], colorL); +void DiscreteCosseratMapping::draw(const sofa::core::visual::VisualParams *vparams) +{ + if (!vparams->displayFlags().getShowMechanicalMappings()) + return; + + // draw cable + typedef RGBAColor RGBAColor; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + const OutDataVecCoord *xfromData = + m_toModel->read(sofa::core::ConstVecCoordId::position()); + const OutVecCoord xData = xfromData->getValue(); + vector positions; + vector> Orientation; + positions.clear(); + Orientation.clear(); + unsigned int sz = xData.size(); + for (unsigned int i = 0; i < sz; i++) { + positions.push_back(xData[i].getCenter()); + Orientation.push_back(xData[i].getOrientation()); } - } else { - int j = 0; - vector index = d_index.getValue(); - for (unsigned int i = 0; i < sz - 1; i++) { - j = m_indicesVectorsDraw[i] - - 1; // to get the articulation on which the frame is related to - RGBAColor color = _eval(xPos[j][d_deformationAxis.getValue()]); - vparams->drawTool()->drawLine(positions[i], positions[i + 1], color); + + // Get access articulated + const In1DataVecCoord *artiData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord xPos = artiData->getValue(); + + RGBAColor drawColor = d_color.getValue(); + // draw each segment of the beam as a cylinder. + for (unsigned int i = 0; i < sz - 1; i++) + vparams->drawTool()->drawCylinder(positions[i], positions[i + 1], + d_radius.getValue(), drawColor); + + // Define color map + SReal min = d_min.getValue(); + SReal max = d_max.getValue(); + sofa::helper::ColorMap::evaluator _eval = + m_colorMap.getEvaluator(min, max); + + glLineWidth(d_radius.getValue()); + glBegin(GL_LINES); + if (d_drawMapBeam.getValue()) { + sofa::type::RGBAColor _color = d_color.getValue(); + RGBAColor colorL = RGBAColor(_color[0], _color[1], _color[2], _color[3]); + glColor4f(colorL[0], colorL[1], colorL[2], colorL[3]); + for (unsigned int i = 0; i < sz - 1; i++) { + vparams->drawTool()->drawLine(positions[i], positions[i + 1], colorL); + } + } else { + int j = 0; + vector index = d_index.getValue(); + for (unsigned int i = 0; i < sz - 1; i++) { + j = m_indicesVectorsDraw[i] - + 1; // to get the articulation on which the frame is related to + RGBAColor color = _eval(xPos[j][d_deformationAxis.getValue()]); + vparams->drawTool()->drawLine(positions[i], positions[i + 1], color); + } } - } - glLineWidth(1); - if (!vparams->displayFlags().getShowMappings()) - if (!d_debug.getValue()) - return; - glEnd(); + glLineWidth(1); + if (!vparams->displayFlags().getShowMappings()) + if (!d_debug.getValue()) + return; + glEnd(); } } // namespace Cosserat::mapping diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h index 94f51f3..272bdfb 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h @@ -145,7 +145,7 @@ class DiscreteDynamicCosseratMapping : public BaseCosseratMapping::applyJT( dataMatOut2Const[0]->endEdit(); } -template -void DiscreteDynamicCosseratMapping::draw(const sofa::core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowMappings()) - return; -} - } // namespace sofa::component::mapping diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.h b/src/Cosserat/mapping/LegendrePolynomialsMapping.h index 102c2e7..6edb32f 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.h +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.h @@ -81,8 +81,6 @@ class LegendrePolynomialsMapping : public core::Mapping void applyJT(const core::ConstraintParams *cparams, DataMatrixDeriv_t& out, const DataMatrixDeriv_t& in) override; - void draw(const core::visual::VisualParams* vparams) override; - }; #if !defined(SOFA_COSSERAT_CPP_LegendrePolynomialsMapping) diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl index 1b87630..197928c 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl @@ -178,12 +178,4 @@ void LegendrePolynomialsMapping::applyJT(const core::ConstraintParams } } - -template -void LegendrePolynomialsMapping::draw(const core::visual::VisualParams* /*vparams*/) -{ - // draw cable - -} - }