diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 227d517..3c598a7 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -397,7 +397,7 @@ void BaseCosseratMapping::computeTangExp(double &curv_abs_n, Mat6x6 &TgX) { if constexpr( Coord1::static_size == 3 ) - computeTangExpImplementation(curv_abs_n, Vec6(strain_i(0),strain_i(1),strain_i(2),0,0,0), TgX); + computeTangExpImplementation(curv_abs_n, Vec6(strain_i(0),strain_i(1),strain_i(2),1.0,0,0), TgX); else computeTangExpImplementation(curv_abs_n, strain_i, TgX); } diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index 85a0517..b8a4373 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -125,6 +125,8 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { void computeBBox(const sofa::core::ExecParams *params, bool onlyVisible) override; void computeLogarithm(const double &x, const Mat4x4 &gX, Mat4x4 &log_gX); + Mat3x3 getTildeMatrix(const Vec3 &u); + protected: ////////////////////////// Inherited attributes //////////////////////////// /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index ac1011d..48f029f 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -326,14 +326,21 @@ void DiscreteCosseratMapping::applyJT( const sofa::VecCoord_t x1from = x1fromData->getValue(); vector local_F_Vec; local_F_Vec.clear(); + vector global_F_Vec; + global_F_Vec.clear(); out1.resize(x1from.size()); // convert the input from Deriv type to vec6 type, for the purpose of the // matrix vector multiplication + Vec6 vec; for (unsigned int var = 0; var < in.size(); ++var) { - Vec6 vec; - for (unsigned j = 0; j < 6; j++) + + vec = Vec6(in[var][3],in[var][4],in[var][5],in[var][0],in[var][1],in[var][2]); + + global_F_Vec.push_back(vec); + + /*for (unsigned j = 0; j < 6; j++) vec[j] = in[var][j]; // Convert input from global frame(SOFA) to local frame const auto _T = @@ -341,7 +348,7 @@ void DiscreteCosseratMapping::applyJT( Mat6x6 P_trans = (this->buildProjector(_T)); P_trans.transpose(); Vec6 local_F = P_trans * vec; - local_F_Vec.push_back(local_F); + local_F_Vec.push_back(local_F);*/ } // Compute output forces @@ -360,7 +367,8 @@ void DiscreteCosseratMapping::applyJT( matB_trans[k][k] = 1.0; for (auto s = sz; s--;) { - Mat6x6 coAdjoint; + + /*Mat6x6 coAdjoint; this->computeCoAdjoint( m_framesExponentialSE3Vectors[s], @@ -370,15 +378,38 @@ void DiscreteCosseratMapping::applyJT( m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in // applyJ (here we transpose) temp.transpose(); - Vec3 f = matB_trans * temp * node_F_Vec; + Vec3 f = matB_trans * temp * node_F_Vec;*/ + + Mat6x6 temp = + m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in + // applyJ (here we transpose) + temp.transpose(); + Vec3 f = matB_trans * temp * global_F_Vec[s]; + + std::cout<<"Global F = " << global_F_Vec[s] <<" at s = " << s <