diff --git a/Sofa/Component/Collision/Response/Contact/CMakeLists.txt b/Sofa/Component/Collision/Response/Contact/CMakeLists.txt index 3b6e2a38162..9ea5c096b4d 100644 --- a/Sofa/Component/Collision/Response/Contact/CMakeLists.txt +++ b/Sofa/Component/Collision/Response/Contact/CMakeLists.txt @@ -6,6 +6,10 @@ set(SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR "src/sofa/component/collisi set(HEADER_FILES ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/config.h.in ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/init.h + ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.h + ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.inl + ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BaseUnilateralContactResponse.h + ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BaseUnilateralContactResponse.inl ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.h ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.inl ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricStickContact.h @@ -26,6 +30,7 @@ set(HEADER_FILES set(SOURCE_FILES ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/init.cpp + ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.cpp ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.cpp ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricStickContact.cpp ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/CollisionResponse.cpp @@ -36,6 +41,7 @@ set(SOURCE_FILES ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/RayContact.cpp ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/RuleBasedContactManager.cpp ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/StickContactConstraint.cpp + ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronAugmentedLagrangianContact.cpp ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronBarycentricPenalityContact.cpp ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronFrictionContact.cpp ${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronRayContact.cpp diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.cpp b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.cpp new file mode 100644 index 00000000000..18a25ba3871 --- /dev/null +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.cpp @@ -0,0 +1,85 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include +#include +#include + +#include +#include + +namespace sofa::component::collision::response::contact +{ + +using namespace defaulttype; +using namespace sofa::helper; +using namespace sofa::component::collision::geometry; +using simulation::Node; + +Creator, PointCollisionModel> > PointPointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, SphereCollisionModel> > LineSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, PointCollisionModel> > LinePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, LineCollisionModel> > LineLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, SphereCollisionModel> > TriangleSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, PointCollisionModel> > TrianglePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, LineCollisionModel> > TriangleLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, TriangleCollisionModel> > TriangleTriangleAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, SphereCollisionModel> > SphereSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, PointCollisionModel> > SpherePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator > RigidSphereRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, RigidSphereModel> > SphereRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, RigidSphereModel> > LineRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator, RigidSphereModel> > TriangleRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator> > RigidSpherePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); + +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, PointCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, SphereCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, PointCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, LineCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, SphereCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, PointCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, LineCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, TriangleCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, SphereCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, PointCollisionModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; + +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, PointCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, SphereCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, PointCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, LineCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, SphereCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, PointCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, LineCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, TriangleCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, SphereCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, PointCollisionModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, RigidSphereModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, RigidSphereModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse, RigidSphereModel>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse>; + +} //namespace sofa::component::collision::response::contact diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.h b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.h new file mode 100644 index 00000000000..052c4d1133f --- /dev/null +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.h @@ -0,0 +1,65 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace sofa::component::collision::response::contact +{ +template +class AugmentedLagrangianResponse : public BaseUnilateralContactResponse +{ +public: + SOFA_CLASS(SOFA_TEMPLATE3(AugmentedLagrangianResponse, TCollisionModel1, TCollisionModel2, ResponseDataTypes), SOFA_TEMPLATE4(BaseUnilateralContactResponse, TCollisionModel1, TCollisionModel2,constraint::lagrangian::model::AugmentedLagrangianContactParameters, ResponseDataTypes)); + + typedef typename Inherit1::DataTypes1 DataTypes1; + typedef typename Inherit1::DataTypes2 DataTypes2; + typedef typename Inherit1::CollisionModel1 CollisionModel1; + typedef typename Inherit1::CollisionModel2 CollisionModel2; + typedef typename Inherit1::Intersection Intersection; + + typedef core::behavior::MechanicalState MechanicalState1; + typedef core::behavior::MechanicalState MechanicalState2; + + Data d_mu; ///< friction parameter + Data d_epsilon; ///< Penalty parameter + + AugmentedLagrangianResponse(); + AugmentedLagrangianResponse(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod); + + virtual ~AugmentedLagrangianResponse() = default; + + virtual constraint::lagrangian::model::AugmentedLagrangianContactParameters getParameterFromDatas() const override; + virtual void setupConstraint(MechanicalState1 *,MechanicalState2 *) override; + +}; + +} // namespace sofa::component::collision::response::contact diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.inl b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.inl new file mode 100644 index 00000000000..7c94ccfd7a0 --- /dev/null +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.inl @@ -0,0 +1,65 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::collision::response::contact +{ + +template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > +AugmentedLagrangianResponse::AugmentedLagrangianResponse() + : AugmentedLagrangianResponse(nullptr, nullptr, nullptr) +{ +} + + +template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > +AugmentedLagrangianResponse::AugmentedLagrangianResponse(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod) + : BaseUnilateralContactResponse(model1,model2,intersectionMethod) + , d_mu (initData(&d_mu, 0.0, "mu", "friction coefficient (0 for frictionless contacts)")) + , d_epsilon (initData(&d_epsilon, 0.0, "epsilon", "Penalty parameter")) +{ + +} + +template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > +constraint::lagrangian::model::AugmentedLagrangianContactParameters AugmentedLagrangianResponse::getParameterFromDatas() const +{ + return {d_mu.getValue(),d_epsilon.getValue()}; +} + + +template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > +void AugmentedLagrangianResponse::setupConstraint(MechanicalState1 * mmodel1,MechanicalState2 * mmodel2) +{ + this->m_constraint = sofa::core::objectmodel::New >(mmodel1, mmodel2); +} + + +} //namespace sofa::component::collision::response::contact diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/BaseUnilateralContactResponse.h b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/BaseUnilateralContactResponse.h new file mode 100644 index 00000000000..272201d37b5 --- /dev/null +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/BaseUnilateralContactResponse.h @@ -0,0 +1,101 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace sofa::component::collision::response::contact +{ +template +class BaseUnilateralContactResponse : public core::collision::Contact, public ContactIdentifier +{ +public: + SOFA_CLASS(SOFA_TEMPLATE4(BaseUnilateralContactResponse, TCollisionModel1, TCollisionModel2,ConstraintParameters, ResponseDataTypes), core::collision::Contact); + typedef TCollisionModel1 CollisionModel1; + typedef TCollisionModel2 CollisionModel2; + typedef core::collision::Intersection Intersection; + typedef typename TCollisionModel1::DataTypes::CPos TVec1; + typedef typename TCollisionModel1::DataTypes::CPos TVec2; + typedef sofa::defaulttype::StdVectorTypes DataTypes1; + typedef sofa::defaulttype::StdVectorTypes DataTypes2; + + typedef core::behavior::MechanicalState MechanicalState1; + typedef core::behavior::MechanicalState MechanicalState2; + typedef typename CollisionModel1::Element CollisionElement1; + typedef typename CollisionModel2::Element CollisionElement2; + typedef core::collision::DetectionOutputVector OutputVector; + typedef core::collision::TDetectionOutputVector TOutputVector; + +protected: + CollisionModel1* model1; + CollisionModel2* model2; + Intersection* intersectionMethod; + bool selfCollision; ///< true if model1==model2 (in this case, only mapper1 is used) + mapper::ContactMapper mapper1; + mapper::ContactMapper mapper2; + + typename constraint::lagrangian::model::BaseContactLagrangianConstraint::SPtr m_constraint; + core::objectmodel::BaseContext* parent; + + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_COLLISION_RESPONSE_CONTACT() + sofa::core::objectmodel::RenamedData mu; + + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_COLLISION_RESPONSE_CONTACT() + sofa::core::objectmodel::RenamedData tol; + + Data d_tol; ///< tolerance for the constraints resolution (0 for default tolerance) + std::vector< sofa::core::collision::DetectionOutput* > contacts; + std::vector< std::pair< std::pair, double > > mappedContacts; + + virtual void activateMappers(); + + void setInteractionTags(MechanicalState1* mstate1, MechanicalState2* mstate2); + + BaseUnilateralContactResponse(); + BaseUnilateralContactResponse(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod); + + ~BaseUnilateralContactResponse() override; +public: + void cleanup() override; + + std::pair getCollisionModels() override { return std::make_pair(model1,model2); } + + void setDetectionOutputs(OutputVector* outputs) override; + + void createResponse(core::objectmodel::BaseContext* group) override; + + void removeResponse() override; + + virtual ConstraintParameters getParameterFromDatas() const = 0; + virtual void setupConstraint(MechanicalState1 *,MechanicalState2 *) = 0; +}; + +} // namespace sofa::component::collision::response::contact diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/BaseUnilateralContactResponse.inl b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/BaseUnilateralContactResponse.inl new file mode 100644 index 00000000000..31b2199f8c0 --- /dev/null +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/BaseUnilateralContactResponse.inl @@ -0,0 +1,271 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::collision::response::contact +{ + +template +BaseUnilateralContactResponse::BaseUnilateralContactResponse() + : BaseUnilateralContactResponse(nullptr, nullptr, nullptr) +{ +} + + +template +BaseUnilateralContactResponse::BaseUnilateralContactResponse(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod) + : model1(model1) + , model2(model2) + , intersectionMethod(intersectionMethod) + , m_constraint(nullptr) + , parent(nullptr) + , d_tol (initData(&d_tol, 0.0, "tol", "tolerance for the constraints resolution (0 for default tolerance)")) +{ + selfCollision = ((core::CollisionModel*)model1 == (core::CollisionModel*)model2); + mapper1.setCollisionModel(model1); + if (!selfCollision) mapper2.setCollisionModel(model2); + contacts.clear(); + mappedContacts.clear(); + + tol.setOriginalData(&d_tol); + +} + +template +BaseUnilateralContactResponse::~BaseUnilateralContactResponse() +{ +} + +template +void BaseUnilateralContactResponse::cleanup() +{ + if (m_constraint) + { + m_constraint->cleanup(); + + if (parent != nullptr) + parent->removeObject(m_constraint); + + parent = nullptr; + m_constraint.reset(); + + mapper1.cleanup(); + + if (!selfCollision) + mapper2.cleanup(); + } + + contacts.clear(); + mappedContacts.clear(); +} + + +template +void BaseUnilateralContactResponse::setDetectionOutputs(OutputVector* o) +{ + TOutputVector& outputs = *static_cast(o); + // We need to remove duplicate contacts + constexpr double minDist2 = 0.00000001f; + + contacts.clear(); + + if (model1->getContactStiffness(0) == 0 || model2->getContactStiffness(0) == 0) + { + msg_error() << "Disabled BaseUnilateralContactResponse with " << (outputs.size()) << " collision points."; + return; + } + + contacts.reserve(outputs.size()); + + const int SIZE = outputs.size(); + + // the following procedure cancels the duplicated detection outputs + for (int cpt=0; cptpoint[0]-p->point[0]).norm2()+(detectionOutput->point[1]-p->point[1]).norm2() < minDist2) + found = true; + } + + if (!found) + contacts.push_back(detectionOutput); + } + + // DUPLICATED CONTACTS FOUND + msg_info_when(contacts.size() +void BaseUnilateralContactResponse::activateMappers() +{ + if (!m_constraint) + { + // Get the mechanical model from mapper1 to fill the constraint vector + MechanicalState1* mmodel1 = mapper1.createMapping(getName().c_str()); + // Get the mechanical model from mapper2 to fill the constraints vector + MechanicalState2* mmodel2; + if (selfCollision) + { + mmodel2 = mmodel1; + } + else + { + mmodel2 = mapper2.createMapping(getName().c_str()); + } + setupConstraint(mmodel1,mmodel2); + m_constraint->setName( getName() ); + setInteractionTags(mmodel1, mmodel2); + m_constraint->setCustomTolerance(d_tol.getValue() ); + } + + int size = contacts.size(); + m_constraint->clear(size); + if (selfCollision) + mapper1.resize(2*size); + else + { + mapper1.resize(size); + mapper2.resize(size); + } + int i = 0; + const double d0 = intersectionMethod->getContactDistance() + model1->getProximity() + model2->getProximity(); // - 0.001; + + mappedContacts.resize(contacts.size()); + for (std::vector::const_iterator it = contacts.begin(); it!=contacts.end(); it++, i++) + { + sofa::core::collision::DetectionOutput* o = *it; + CollisionElement1 elem1(o->elem.first); + CollisionElement2 elem2(o->elem.second); + int index1 = elem1.getIndex(); + int index2 = elem2.getIndex(); + + typename DataTypes1::Real r1 = 0.; + typename DataTypes2::Real r2 = 0.; + + // Create mapping for first point + index1 = mapper1.addPointB(o->point[0], index1, r1); + // Create mapping for second point + if (selfCollision) + { + index2 = mapper1.addPointB(o->point[1], index2, r2); + } + else + { + index2 = mapper2.addPointB(o->point[1], index2, r2); + } + const double distance = d0 + r1 + r2; + + mappedContacts[i].first.first = index1; + mappedContacts[i].first.second = index2; + mappedContacts[i].second = distance; + } + + // Update mappings + mapper1.update(); + mapper1.updateXfree(); + if (!selfCollision) mapper2.update(); + if (!selfCollision) mapper2.updateXfree(); +} + +template +void BaseUnilateralContactResponse::createResponse(core::objectmodel::BaseContext* group) +{ + + activateMappers(); + + if (m_constraint) + { + int i=0; + for (std::vector::const_iterator it = contacts.begin(); it!=contacts.end(); it++, i++) + { + const sofa::core::collision::DetectionOutput* o = *it; + const int index1 = mappedContacts[i].first.first; + const int index2 = mappedContacts[i].first.second; + const double distance = mappedContacts[i].second; + + // Polynome de Cantor de NxN sur N bijectif f(x,y)=((x+y)^2+3x+y)/2 + const long index = cantorPolynomia(o->id /*cantorPolynomia(index1, index2)*/,id); + + const ConstraintParameters params = getParameterFromDatas(); + + // Add contact in unilateral constraint + m_constraint->addContact(params, o->normal, distance, index1, index2, index, o->id); + } + + if (parent!=nullptr) + { + parent->removeObject(this); + parent->removeObject(m_constraint); + } + + parent = group; + if (parent!=nullptr) + { + parent->addObject(this); + parent->addObject(m_constraint); + } + } +} + +template +void BaseUnilateralContactResponse::removeResponse() +{ + if (m_constraint) + { + mapper1.resize(0); + mapper2.resize(0); + if (parent!=nullptr) + { + parent->removeObject(this); + parent->removeObject(m_constraint); + } + parent = nullptr; + } +} + +template +void BaseUnilateralContactResponse::setInteractionTags(MechanicalState1* mstate1, MechanicalState2* mstate2) +{ + sofa::core::objectmodel::TagSet tagsm1 = mstate1->getTags(); + sofa::core::objectmodel::TagSet tagsm2 = mstate2->getTags(); + sofa::core::objectmodel::TagSet::iterator it; + for(it=tagsm1.begin(); it != tagsm1.end(); it++) + m_constraint->addTag(*it); + for(it=tagsm2.begin(); it!=tagsm2.end(); it++) + m_constraint->addTag(*it); +} + +} //namespace sofa::component::collision::response::contact diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.cpp b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.cpp index e405e4ff362..a3734fd4d83 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.cpp +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.cpp @@ -19,8 +19,12 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ +#include +#include +#include #include + #include #include @@ -32,6 +36,7 @@ using namespace sofa::helper; using namespace sofa::component::collision::geometry; using simulation::Node; + Creator, PointCollisionModel> > PointPointFrictionContactClass("FrictionContactConstraint",true); Creator, SphereCollisionModel> > LineSphereFrictionContactClass("FrictionContactConstraint",true); Creator, PointCollisionModel> > LinePointFrictionContactClass("FrictionContactConstraint",true); @@ -48,6 +53,23 @@ Creator, RigidSphereModel> > TriangleRigidSphereFrictionContactClass("FrictionContactConstraint",true); Creator> > RigidSpherePointFrictionContactClass("FrictionContactConstraint",true); + +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, PointCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, SphereCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, PointCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, LineCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, SphereCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, PointCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, LineCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, TriangleCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, SphereCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, PointCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, RigidSphereModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, RigidSphereModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse, RigidSphereModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; + template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API FrictionContact, PointCollisionModel>; template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API FrictionContact, SphereCollisionModel>; template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API FrictionContact, PointCollisionModel>; diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h index cebd6506a02..999831d9721 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h @@ -27,7 +27,7 @@ #include #include #include -#include +#include #include #include @@ -35,65 +35,30 @@ namespace sofa::component::collision::response::contact { template -class FrictionContact : public core::collision::Contact, public ContactIdentifier +class FrictionContact : public BaseUnilateralContactResponse { -public: - SOFA_CLASS(SOFA_TEMPLATE3(FrictionContact, TCollisionModel1, TCollisionModel2, ResponseDataTypes), core::collision::Contact); - typedef TCollisionModel1 CollisionModel1; - typedef TCollisionModel2 CollisionModel2; - typedef core::collision::Intersection Intersection; - typedef typename TCollisionModel1::DataTypes::CPos TVec1; - typedef typename TCollisionModel1::DataTypes::CPos TVec2; - typedef sofa::defaulttype::StdVectorTypes DataTypes1; - typedef sofa::defaulttype::StdVectorTypes DataTypes2; + public: + SOFA_CLASS(SOFA_TEMPLATE3(FrictionContact, TCollisionModel1, TCollisionModel2, ResponseDataTypes), SOFA_TEMPLATE4(BaseUnilateralContactResponse, TCollisionModel1, TCollisionModel2,constraint::lagrangian::model::UnilateralLagrangianContactParameters, ResponseDataTypes)); + + typedef typename Inherit1::DataTypes1 DataTypes1; + typedef typename Inherit1::DataTypes2 DataTypes2; + typedef typename Inherit1::CollisionModel1 CollisionModel1; + typedef typename Inherit1::CollisionModel2 CollisionModel2; + typedef typename Inherit1::Intersection Intersection; typedef core::behavior::MechanicalState MechanicalState1; typedef core::behavior::MechanicalState MechanicalState2; - typedef typename CollisionModel1::Element CollisionElement1; - typedef typename CollisionModel2::Element CollisionElement2; - typedef core::collision::DetectionOutputVector OutputVector; - typedef core::collision::TDetectionOutputVector TOutputVector; - -protected: - CollisionModel1* model1; - CollisionModel2* model2; - Intersection* intersectionMethod; - bool selfCollision; ///< true if model1==model2 (in this case, only mapper1 is used) - mapper::ContactMapper mapper1; - mapper::ContactMapper mapper2; - - constraint::lagrangian::model::UnilateralLagrangianConstraint::SPtr m_constraint; - core::objectmodel::BaseContext* parent; - - SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_COLLISION_RESPONSE_CONTACT() - sofa::core::objectmodel::RenamedData mu; - - SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_COLLISION_RESPONSE_CONTACT() - sofa::core::objectmodel::RenamedData tol; - Data d_mu; ///< friction coefficient (0 for frictionless contacts) - Data d_tol; ///< tolerance for the constraints resolution (0 for default tolerance) - std::vector< sofa::core::collision::DetectionOutput* > contacts; - std::vector< std::pair< std::pair, double > > mappedContacts; - - virtual void activateMappers(); - - void setInteractionTags(MechanicalState1* mstate1, MechanicalState2* mstate2); + Data d_mu; ///< friction parameter FrictionContact(); FrictionContact(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod); - ~FrictionContact() override; -public: - void cleanup() override; - - std::pair getCollisionModels() override { return std::make_pair(model1,model2); } - - void setDetectionOutputs(OutputVector* outputs) override; + virtual ~FrictionContact() = default; - void createResponse(core::objectmodel::BaseContext* group) override; + virtual constraint::lagrangian::model::UnilateralLagrangianContactParameters getParameterFromDatas() const override; + virtual void setupConstraint(MechanicalState1 *,MechanicalState2 *) override; - void removeResponse() override; }; } // namespace sofa::component::collision::response::contact diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl index a7fdd1fcce1..6ee9fe777ed 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl @@ -41,235 +41,24 @@ FrictionContact::FrictionCo template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > FrictionContact::FrictionContact(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod) - : model1(model1) - , model2(model2) - , intersectionMethod(intersectionMethod) - , m_constraint(nullptr) - , parent(nullptr) - , d_mu (initData(&d_mu, 0.8, "mu", "friction coefficient (0 for frictionless contacts)")) - , d_tol (initData(&d_tol, 0.0, "tol", "tolerance for the constraints resolution (0 for default tolerance)")) + : BaseUnilateralContactResponse(model1,model2,intersectionMethod) + , d_mu (initData(&d_mu, 0.8, "mu", "friction coefficient (0 for frictionless contacts)")) { - selfCollision = ((core::CollisionModel*)model1 == (core::CollisionModel*)model2); - mapper1.setCollisionModel(model1); - if (!selfCollision) mapper2.setCollisionModel(model2); - contacts.clear(); - mappedContacts.clear(); - mu.setOriginalData(&d_mu); - tol.setOriginalData(&d_tol); - -} - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -FrictionContact::~FrictionContact() -{ -} - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void FrictionContact::cleanup() -{ - if (m_constraint) - { - m_constraint->cleanup(); - - if (parent != nullptr) - parent->removeObject(m_constraint); - - parent = nullptr; - m_constraint.reset(); - - mapper1.cleanup(); - - if (!selfCollision) - mapper2.cleanup(); - } - - contacts.clear(); - mappedContacts.clear(); -} - - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void FrictionContact::setDetectionOutputs(OutputVector* o) -{ - TOutputVector& outputs = *static_cast(o); - // We need to remove duplicate contacts - constexpr double minDist2 = 0.00000001f; - - contacts.clear(); - - if (model1->getContactStiffness(0) == 0 || model2->getContactStiffness(0) == 0) - { - msg_error() << "Disabled FrictionContact with " << (outputs.size()) << " collision points."; - return; - } - - contacts.reserve(outputs.size()); - - const int SIZE = outputs.size(); - - // the following procedure cancels the duplicated detection outputs - for (int cpt=0; cptpoint[0]-p->point[0]).norm2()+(detectionOutput->point[1]-p->point[1]).norm2() < minDist2) - found = true; - } - - if (!found) - contacts.push_back(detectionOutput); - } - - // DUPLICATED CONTACTS FOUND - msg_info_when(contacts.size() -void FrictionContact::activateMappers() +constraint::lagrangian::model::UnilateralLagrangianContactParameters FrictionContact::getParameterFromDatas() const { - if (!m_constraint) - { - // Get the mechanical model from mapper1 to fill the constraint vector - MechanicalState1* mmodel1 = mapper1.createMapping(getName().c_str()); - // Get the mechanical model from mapper2 to fill the constraints vector - MechanicalState2* mmodel2; - if (selfCollision) - { - mmodel2 = mmodel1; - } - else - { - mmodel2 = mapper2.createMapping(getName().c_str()); - } - m_constraint = sofa::core::objectmodel::New >(mmodel1, mmodel2); - m_constraint->setName( getName() ); - setInteractionTags(mmodel1, mmodel2); - m_constraint->setCustomTolerance(d_tol.getValue() ); - } - - int size = contacts.size(); - m_constraint->clear(size); - if (selfCollision) - mapper1.resize(2*size); - else - { - mapper1.resize(size); - mapper2.resize(size); - } - int i = 0; - const double d0 = intersectionMethod->getContactDistance() + model1->getProximity() + model2->getProximity(); // - 0.001; - - mappedContacts.resize(contacts.size()); - for (std::vector::const_iterator it = contacts.begin(); it!=contacts.end(); it++, i++) - { - sofa::core::collision::DetectionOutput* o = *it; - CollisionElement1 elem1(o->elem.first); - CollisionElement2 elem2(o->elem.second); - int index1 = elem1.getIndex(); - int index2 = elem2.getIndex(); - - typename DataTypes1::Real r1 = 0.; - typename DataTypes2::Real r2 = 0.; - - // Create mapping for first point - index1 = mapper1.addPointB(o->point[0], index1, r1); - // Create mapping for second point - if (selfCollision) - { - index2 = mapper1.addPointB(o->point[1], index2, r2); - } - else - { - index2 = mapper2.addPointB(o->point[1], index2, r2); - } - const double distance = d0 + r1 + r2; - - mappedContacts[i].first.first = index1; - mappedContacts[i].first.second = index2; - mappedContacts[i].second = distance; - } - - // Update mappings - mapper1.update(); - mapper1.updateXfree(); - if (!selfCollision) mapper2.update(); - if (!selfCollision) mapper2.updateXfree(); + return {d_mu.getValue()}; } -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void FrictionContact::createResponse(core::objectmodel::BaseContext* group) -{ - - activateMappers(); - const double mu_ = this->d_mu.getValue(); - // Checks if friction is considered - if ( mu_ < 0.0 ) - msg_error() << "mu has to take positive values"; - - if (m_constraint) - { - int i=0; - for (std::vector::const_iterator it = contacts.begin(); it!=contacts.end(); it++, i++) - { - const sofa::core::collision::DetectionOutput* o = *it; - const int index1 = mappedContacts[i].first.first; - const int index2 = mappedContacts[i].first.second; - const double distance = mappedContacts[i].second; - - // Polynome de Cantor de NxN sur N bijectif f(x,y)=((x+y)^2+3x+y)/2 - const long index = cantorPolynomia(o->id /*cantorPolynomia(index1, index2)*/,id); - - // Add contact in unilateral constraint - m_constraint->addContact(mu_, o->normal, distance, index1, index2, index, o->id); - } - - if (parent!=nullptr) - { - parent->removeObject(this); - parent->removeObject(m_constraint); - } - - parent = group; - if (parent!=nullptr) - { - parent->addObject(this); - parent->addObject(m_constraint); - } - } -} template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void FrictionContact::removeResponse() +void FrictionContact::setupConstraint(MechanicalState1 * mmodel1,MechanicalState2 * mmodel2) { - if (m_constraint) - { - mapper1.resize(0); - mapper2.resize(0); - if (parent!=nullptr) - { - parent->removeObject(this); - parent->removeObject(m_constraint); - } - parent = nullptr; - } + this->m_constraint = sofa::core::objectmodel::New >(mmodel1, mmodel2); } -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void FrictionContact::setInteractionTags(MechanicalState1* mstate1, MechanicalState2* mstate2) -{ - sofa::core::objectmodel::TagSet tagsm1 = mstate1->getTags(); - sofa::core::objectmodel::TagSet tagsm2 = mstate2->getTags(); - sofa::core::objectmodel::TagSet::iterator it; - for(it=tagsm1.begin(); it != tagsm1.end(); it++) - m_constraint->addTag(*it); - for(it=tagsm2.begin(); it!=tagsm2.end(); it++) - m_constraint->addTag(*it); -} } //namespace sofa::component::collision::response::contact diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/TetrahedronAugmentedLagrangianContact.cpp b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/TetrahedronAugmentedLagrangianContact.cpp new file mode 100644 index 00000000000..d5fe12ce865 --- /dev/null +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/TetrahedronAugmentedLagrangianContact.cpp @@ -0,0 +1,60 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include +#include +#include +#include +#include +#include + +using namespace sofa::core::collision; + +namespace sofa::component::collision::response::contact +{ + +using namespace sofa::component::collision::geometry; + +Creator> > TetrahedronSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator> > TetrahedronPointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator> > TetrahedronLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator> > TetrahedronTriangleAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator > TetrahedronTetrahedronAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); + +Creator> > TetrahedronSpherePenalityAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator> > TetrahedronPointPenalityAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator> > TetrahedronLinePenalityAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator> > TetrahedronTrianglePenalityAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); +Creator > TetrahedronTetrahedronPenalityAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true); + +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::AugmentedLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse; + +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::AugmentedLagrangianResponse>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::AugmentedLagrangianResponse>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::AugmentedLagrangianResponse>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::AugmentedLagrangianResponse>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::AugmentedLagrangianResponse; + +} // namespace sofa::component::collision::response::contact diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/TetrahedronFrictionContact.cpp b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/TetrahedronFrictionContact.cpp index 3f20f87b676..53d5265d7bc 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/TetrahedronFrictionContact.cpp +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/TetrahedronFrictionContact.cpp @@ -24,6 +24,7 @@ #include #include #include +#include using namespace sofa::core::collision; @@ -44,6 +45,12 @@ Creator> > TetrahedronTrianglePenalityFrictionContactClass("FrictionContactConstraint",true); Creator > TetrahedronTetrahedronPenalityFrictionContactClass("FrictionContactConstraint",true); +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::BaseUnilateralContactResponse; + template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::FrictionContact>; template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::FrictionContact>; template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API response::contact::FrictionContact>; diff --git a/Sofa/Component/Collision/Response/Contact/tests/CMakeLists.txt b/Sofa/Component/Collision/Response/Contact/tests/CMakeLists.txt index 73c983810c6..8da60705f95 100644 --- a/Sofa/Component/Collision/Response/Contact/tests/CMakeLists.txt +++ b/Sofa/Component/Collision/Response/Contact/tests/CMakeLists.txt @@ -6,7 +6,8 @@ set(SOURCE_FILES PenalityContactForceField_test.cpp ) + add_executable(${PROJECT_NAME} ${SOURCE_FILES}) target_link_libraries(${PROJECT_NAME} Sofa.Testing Sofa.Component.Collision.Testing Sofa.Component.SolidMechanics.Testing) -target_link_libraries(${PROJECT_NAME} Sofa.Component.Collision.Response.Contact) +target_link_libraries(${PROJECT_NAME} Sofa.Component.Collision.Response.Contact Sofa.Component.Constraint.Lagrangian) add_test(NAME ${PROJECT_NAME} COMMAND ${PROJECT_NAME}) diff --git a/Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt b/Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt index 05559157449..07cfdad10db 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt +++ b/Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt @@ -19,6 +19,11 @@ set(HEADER_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralInteractionConstraint.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralInteractionConstraint.inl + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/AugmentedLagrangianResolution.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/AugmentedLagrangianConstraint.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/AugmentedLagrangianConstraint.inl + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BaseContactLagrangianConstraint.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BaseContactLagrangianConstraint.inl ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralLagrangianConstraint.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralLagrangianConstraint.inl ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/FixedLagrangianConstraint.h @@ -35,6 +40,7 @@ set(HEADER_FILES set(SOURCE_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/init.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/AugmentedLagrangianConstraint.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralLagrangianConstraint.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/FixedLagrangianConstraint.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/SlidingLagrangianConstraint.cpp diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.cpp new file mode 100644 index 00000000000..0f2ac2fad2f --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.cpp @@ -0,0 +1,109 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#define SOFA_COMPONENT_CONSTRAINTSET_AugmentedLagrangianConstraint_CPP +#include +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::model +{ + +using namespace sofa::defaulttype; +using namespace sofa::helper; + +int AugmentedLagrangianConstraintClass = core::RegisterObject("AugmentedLagrangianConstraint") + .add< AugmentedLagrangianConstraint >() + + ; + + +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BaseContactLagrangianConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API AugmentedLagrangianConstraint; + + + +void AugmentedLagrangianResolutionWithFriction::init(int line, SReal** w, SReal* force) +{ + _W[0]=w[line ][line ]; + _W[1]=w[line ][line+1]; + _W[2]=w[line ][line+2]; + _W[3]=w[line+1][line+1]; + _W[4]=w[line+1][line+2]; + _W[5]=w[line+2][line+2]; + + ////////////////// christian : the following does not work ! ///////// + if(_prev) + { + force[line] = _prev->popForce(); + force[line+1] = _prev->popForce(); + force[line+2] = _prev->popForce(); + } + +} + +void AugmentedLagrangianResolutionWithFriction::resolution(int line, SReal** /*w*/, SReal* d, SReal* force, SReal * /*dfree*/) +{ + force[line] -= d[line] * _epsilon; + + if(force[line] < 0) + { + force[line]=0; force[line+1]=0; force[line+2]=0; + return; + } + + const SReal f_t_0 = force[line + 1] - d[line+ 1] * _epsilon; + const SReal f_t_1 = force[line + 2] - d[line+ 2] * _epsilon; + + const SReal criteria = sqrt(pow(f_t_0,2.0) + pow(f_t_1,2.0)) - _mu * fabs(force[line]); + + if(criteria<0) + { + force[line+1] = f_t_0 ; + force[line+2] = f_t_1 ; + } + else + { + const SReal norm_s = sqrt(pow(d[line+ 1],2.0) + pow(d[line+ 2],2.0)); + force[line+1] -= _mu * d[line] * _epsilon * d[line+ 1]/norm_s; + force[line+2] -= _mu * d[line] * _epsilon * d[line+ 2]/norm_s; + } +} + +void AugmentedLagrangianResolutionWithFriction::store(int line, SReal* force, bool /*convergence*/) +{ + if(_prev) + { + _prev->pushForce(force[line]); + _prev->pushForce(force[line+1]); + _prev->pushForce(force[line+2]); + } + + if(_active) + { + *_active = (force[line] != 0); + _active = nullptr; // Won't be used in the haptic thread + } +} + + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.h new file mode 100644 index 00000000000..f0f974cd28d --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.h @@ -0,0 +1,77 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::model +{ + +struct AugmentedLagrangianContactParameters : public BaseContactParams +{ + AugmentedLagrangianContactParameters() : mu(0.0), epsilon(0.0) {}; + AugmentedLagrangianContactParameters(SReal _mu, SReal _epsilon) : mu(_mu), epsilon(_epsilon) {}; + + virtual bool hasTangentialComponent() const override + { + return mu>0.0; + } + + SReal mu; + SReal epsilon; +}; + +template +class AugmentedLagrangianConstraint : public BaseContactLagrangianConstraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(AugmentedLagrangianConstraint,DataTypes), SOFA_TEMPLATE2(BaseContactLagrangianConstraint,DataTypes,AugmentedLagrangianContactParameters)); + typedef BaseContactLagrangianConstraint Inherit; + typedef typename Inherit::MechanicalState MechanicalState; + typedef typename Inherit::Contact Contact; + +protected: + AugmentedLagrangianConstraint(MechanicalState* object1=nullptr, MechanicalState* object2=nullptr); + virtual ~AugmentedLagrangianConstraint() = default; + +public: + virtual void getConstraintResolution(const core::ConstraintParams *,std::vector& resTab, unsigned int& offset) override; + +}; + + +#if !defined(SOFA_COMPONENT_CONSTRAINTSET_AugmentedLagrangianConstraint_CPP) + extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BaseContactLagrangianConstraint; + extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API AugmentedLagrangianConstraint; +#endif + + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.inl new file mode 100644 index 00000000000..77f5c19913a --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.inl @@ -0,0 +1,70 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::model +{ + +template +AugmentedLagrangianConstraint::AugmentedLagrangianConstraint(MechanicalState* object1, MechanicalState* object2) + : Inherit(object1, object2) +{ +} + +template +void AugmentedLagrangianConstraint::getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) +{ + if(this->contactsStatus) + { + delete[] this->contactsStatus; + this->contactsStatus = nullptr; + } + + if (this->contacts.size() > 0) + { + this->contactsStatus = new bool[this->contacts.size()]; + memset(this->contactsStatus, 0, sizeof(bool)*this->contacts.size()); + } + + for(unsigned int i=0; icontacts.size(); i++) + { + Contact& c = this->contacts[i]; + if(c.parameters.hasTangentialComponent()) + { + AugmentedLagrangianResolutionWithFriction* ucrwf = new AugmentedLagrangianResolutionWithFriction(c.parameters.mu,c.parameters.epsilon, nullptr, &(this->contactsStatus[i])); + ucrwf->setTolerance(this->customTolerance); + resTab[offset] = ucrwf; + + // TODO : cette méthode de stockage des forces peu mal fonctionner avec 2 threads quand on utilise l'haptique + offset += 3; + } + else + resTab[offset++] = new AugmentedLagrangianResolution(c.parameters.epsilon); + } +} + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianResolution.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianResolution.h new file mode 100644 index 00000000000..78ff8d7bb73 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/AugmentedLagrangianResolution.h @@ -0,0 +1,74 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::constraint::lagrangian::model +{ + +class AugmentedLagrangianResolution : public core::behavior::ConstraintResolution +{ + public: + AugmentedLagrangianResolution(SReal epsilon) : core::behavior::ConstraintResolution(1), m_epsilon(epsilon) {} + + void resolution(int line, SReal** w, SReal* d, SReal* force, SReal* dfree) override + { + SOFA_UNUSED(dfree); + force[line] -= d[line] / w[line][line]; + if (force[line] < 0) force[line] = 0.0; + } +private: + SReal m_epsilon; +}; + + +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API AugmentedLagrangianResolutionWithFriction + : public core::behavior::ConstraintResolution +{ + public: + AugmentedLagrangianResolutionWithFriction(SReal mu, SReal epsilon, PreviousForcesContainer* prev = nullptr, + bool* active = nullptr) + : core::behavior::ConstraintResolution(3), _mu(mu), _epsilon(epsilon), _prev(prev), _active(active) + { + } + + void init(int line, SReal** w, SReal* force) override; + void resolution(int line, SReal** w, SReal* d, SReal* force, SReal* dFree) override; + void store(int line, SReal* force, bool /*convergence*/) override; + + protected: + SReal _mu; + SReal _epsilon; + SReal _W[6]; + PreviousForcesContainer* _prev; + bool* _active; // Will set this after the resolution +}; + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.h new file mode 100644 index 00000000000..512a06ebd5b --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.h @@ -0,0 +1,160 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::model +{ + + +//TODO(Paul) C++20: add concept for ContactParams +// Every ContactParams should implement hasTangentialComponent +struct BaseContactParams +{ + virtual bool hasTangentialComponent() const = 0; +}; + +template +class BaseContactLagrangianConstraint : public core::behavior::PairInteractionConstraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE2(BaseContactLagrangianConstraint,DataTypes,ContactParams), SOFA_TEMPLATE(core::behavior::PairInteractionConstraint,DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::MatrixDeriv::RowConstIterator MatrixDerivRowConstIterator; + typedef typename DataTypes::MatrixDeriv::ColConstIterator MatrixDerivColConstIterator; + typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename DataTypes::MatrixDeriv::ColIterator MatrixDerivColIterator; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + typedef typename core::behavior::MechanicalState MechanicalState; + + typedef core::behavior::BaseConstraint::ConstraintBlockInfo ConstraintBlockInfo; + typedef core::behavior::BaseConstraint::PersistentID PersistentID; + typedef core::behavior::BaseConstraint::ConstCoord ConstCoord; + + typedef core::behavior::BaseConstraint::VecConstraintBlockInfo VecConstraintBlockInfo; + typedef core::behavior::BaseConstraint::VecPersistentID VecPersistentID; + typedef core::behavior::BaseConstraint::VecConstCoord VecConstCoord; + typedef core::behavior::BaseConstraint::VecConstDeriv VecConstDeriv; + typedef core::behavior::BaseConstraint::VecConstArea VecConstArea; + + typedef core::objectmodel::Data DataVecCoord; + typedef core::objectmodel::Data DataVecDeriv; + typedef core::objectmodel::Data DataMatrixDeriv; + + typedef typename core::behavior::PairInteractionConstraint Inherit; + +protected: + + struct Contact + { + int m1, m2; ///< the two extremities of the spring: masses m1 and m2 + Deriv norm; ///< contact normal, from m1 to m2 + Deriv t; ///< added for friction + Deriv s; ///< added for friction + Real contactDistance; + + unsigned int id; + long contactId; + PersistentID localId; + ContactParams parameters; + + Coord P, Q; + + mutable Real dfree; + }; + + sofa::type::vector contacts; + bool yetIntegrated; + SReal customTolerance; + + bool* contactsStatus; + + /// Computes constraint violation in position and stores it into resolution global vector + /// + /// @param v Global resolution vector + virtual void getPositionViolation(linearalgebra::BaseVector *v); + + ///Computes constraint violation in velocity and stores it into resolution global vector + /// + /// @param v Global resolution vector + virtual void getVelocityViolation(linearalgebra::BaseVector *v); + +public: + + unsigned int constraintId; +protected: + + virtual type::vector getUnilateralInteractionIdentifiers() {return {};} + + virtual type::vector getPairInteractionIdentifiers() override final + { + type::vector ids = getUnilateralInteractionIdentifiers(); + ids.push_back("Unilateral"); + return ids; + } + + + BaseContactLagrangianConstraint(MechanicalState* object1=nullptr, MechanicalState* object2=nullptr); + virtual ~BaseContactLagrangianConstraint(); + +public: + void setCustomTolerance(SReal tol) { customTolerance = tol; } + + void clear(int reserve=0); + + + void addContact(const ContactParams& parameters, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord Pfree, Coord Qfree, long id=0, PersistentID localid=0); + void addContact(const ContactParams& parameters, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0); + void addContact(const ContactParams& parameters, Deriv norm, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0); + + void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex + , const DataVecCoord &x1, const DataVecCoord &x2) override; + + void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord &x1, const DataVecCoord &x2 + , const DataVecDeriv &v1, const DataVecDeriv &v2) override; + + + void getConstraintInfo(const core::ConstraintParams* cParams, VecConstraintBlockInfo& blocks, VecPersistentID& ids, VecConstCoord& positions, VecConstDeriv& directions, VecConstArea& areas) override; + + virtual void getConstraintResolution(const core::ConstraintParams *,std::vector& resTab, unsigned int& offset) =0; + bool isActive() const override; + + void draw(const core::visual::VisualParams* vparams) override; +}; + + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.inl new file mode 100644 index 00000000000..5286a588fd3 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.inl @@ -0,0 +1,388 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::model +{ + +template +BaseContactLagrangianConstraint::BaseContactLagrangianConstraint(MechanicalState* object1, MechanicalState* object2) + : Inherit(object1, object2) + , yetIntegrated(false) + , customTolerance(0.0) + , contactsStatus(nullptr) +{ +} + +template +BaseContactLagrangianConstraint::~BaseContactLagrangianConstraint() +{ + if(contactsStatus) + delete[] contactsStatus; +} + +template +void BaseContactLagrangianConstraint::clear(int reserve) +{ + contacts.clear(); + if (reserve) + contacts.reserve(reserve); +} + +template +void BaseContactLagrangianConstraint::addContact(const ContactParams& parameters, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id, PersistentID localid) +{ + addContact(parameters, norm, P, Q, contactDistance, m1, m2, + this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2], + this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1], + id, localid); +} + +template +void BaseContactLagrangianConstraint::addContact(const ContactParams& parameters, Deriv norm, Real contactDistance, int m1, int m2, long id, PersistentID localid) +{ + addContact(parameters, norm, + this->getMState2()->read(core::ConstVecCoordId::position())->getValue()[m2], + this->getMState1()->read(core::ConstVecCoordId::position())->getValue()[m1], + contactDistance, m1, m2, + this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2], + this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1], + id, localid); +} + +template +void BaseContactLagrangianConstraint::addContact(const ContactParams& parameters, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord /*Pfree*/, Coord /*Qfree*/, long id, PersistentID localid) +{ + contacts.resize(contacts.size() + 1); + Contact &c = contacts.back(); + + c.P = P; + c.Q = Q; + c.m1 = m1; + c.m2 = m2; + c.norm = norm; + c.t = Deriv(norm.z(), norm.x(), norm.y()); + c.s = cross(norm, c.t); + c.s = c.s / c.s.norm(); + c.t = cross((-norm), c.s); + c.parameters= parameters; + c.contactId = id; + c.localId = localid; + c.contactDistance = contactDistance; +} + + +template +void BaseContactLagrangianConstraint::buildConstraintMatrix(const core::ConstraintParams *, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &contactId + , const DataVecCoord &, const DataVecCoord &) +{ + assert(this->mstate1); + assert(this->mstate2); + + if (this->mstate1 == this->mstate2) + { + MatrixDeriv& c1 = *c1_d.beginEdit(); + + for (unsigned int i = 0; i < contacts.size(); i++) + { + Contact& c = contacts[i]; + + c.id = contactId++; + + MatrixDerivRowIterator c1_it = c1.writeLine(c.id); + + c1_it.addCol(c.m1, -c.norm); + c1_it.addCol(c.m2, c.norm); + + if (c.parameters.hasTangentialComponent()) + { + c1_it = c1.writeLine(c.id + 1); + c1_it.setCol(c.m1, -c.t); + c1_it.setCol(c.m2, c.t); + + c1_it = c1.writeLine(c.id + 2); + c1_it.setCol(c.m1, -c.s); + c1_it.setCol(c.m2, c.s); + + contactId += 2; + } + } + + c1_d.endEdit(); + } + else + { + MatrixDeriv& c1 = *c1_d.beginEdit(); + MatrixDeriv& c2 = *c2_d.beginEdit(); + + for (unsigned int i = 0; i < contacts.size(); i++) + { + Contact& c = contacts[i]; + + c.id = contactId++; + + MatrixDerivRowIterator c1_it = c1.writeLine(c.id); + c1_it.addCol(c.m1, -c.norm); + + MatrixDerivRowIterator c2_it = c2.writeLine(c.id); + c2_it.addCol(c.m2, c.norm); + + if (c.parameters.hasTangentialComponent()) + { + c1_it = c1.writeLine(c.id + 1); + c1_it.setCol(c.m1, -c.t); + + c1_it = c1.writeLine(c.id + 2); + c1_it.setCol(c.m1, -c.s); + + c2_it = c2.writeLine(c.id + 1); + c2_it.setCol(c.m2, c.t); + + c2_it = c2.writeLine(c.id + 2); + c2_it.setCol(c.m2, c.s); + + contactId += 2; + } + } + + c1_d.endEdit(); + c2_d.endEdit(); + } +} + + +template +void BaseContactLagrangianConstraint::getPositionViolation(linearalgebra::BaseVector *v) +{ + const VecCoord &PfreeVec = this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue(); + const VecCoord &QfreeVec = this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue(); + + Real dfree = (Real)0.0; + Real dfree_t = (Real)0.0; + Real dfree_s = (Real)0.0; + + const unsigned int cSize = contacts.size(); + + for (unsigned int i = 0; i < cSize; i++) + { + const Contact& c = contacts[i]; + + // Compute dfree, dfree_t and d_free_s + + const Coord &Pfree = PfreeVec[c.m2]; + const Coord &Qfree = QfreeVec[c.m1]; + + const Coord PPfree = Pfree - c.P; + const Coord QQfree = Qfree - c.Q; + + const Real ref_dist = PPfree.norm() + QQfree.norm(); + + dfree = dot(Pfree - Qfree, c.norm) - c.contactDistance; + const Real delta = dot(c.P - c.Q, c.norm) - c.contactDistance; + + if ((helper::rabs(delta) < 0.00001 * ref_dist) && (helper::rabs(dfree) < 0.00001 * ref_dist)) + { + dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); + dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); + } + else if (helper::rabs(delta - dfree) > 0.001 * delta) + { + const Real dt = delta / (delta - dfree); + + if (dt > 0.0 && dt < 1.0) + { + const Coord Pt = c.P * (1 - dt) + Pfree * dt; + const Coord Qt = c.Q * (1 - dt) + Qfree * dt; + const Coord PtPfree = Pfree - Pt; + const Coord QtQfree = Qfree - Qt; + + dfree_t = dot(PtPfree, c.t) - dot(QtQfree, c.t); + dfree_s = dot(PtPfree, c.s) - dot(QtQfree, c.s); + } + else if (dfree < 0.0) + { + dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); + dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); + } + else + { + dfree_t = 0; + dfree_s = 0; + } + } + else + { + dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); + dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); + } + + // Sets dfree in global violation vector + + v->set(c.id, dfree); + + c.dfree = dfree; // PJ : For isActive() method. Don't know if it's still usefull. + + if (c.parameters.hasTangentialComponent()) + { + v->set(c.id + 1, dfree_t); + v->set(c.id + 2, dfree_s); + } + } +} + + +template +void BaseContactLagrangianConstraint::getVelocityViolation(linearalgebra::BaseVector *v) +{ + auto P = this->getMState2()->readPositions(); + auto Q = this->getMState1()->readPositions(); + + const SReal dt = this->getContext()->getDt(); + const SReal invDt = SReal(1.0) / dt; + + const VecDeriv &PvfreeVec = this->getMState2()->read(core::ConstVecDerivId::freeVelocity())->getValue(); + const VecDeriv &QvfreeVec = this->getMState1()->read(core::ConstVecDerivId::freeVelocity())->getValue(); + + const unsigned int cSize = contacts.size(); + + for (unsigned int i = 0; i < cSize; i++) + { + const Contact& c = contacts[i]; + + const Deriv QP_invDt = (P[c.m2] - Q[c.m1])*invDt; + const Deriv QP_vfree = PvfreeVec[c.m2] - QvfreeVec[c.m1]; + const Deriv dFreeVec = QP_vfree + QP_invDt; + + v->set(c.id, dot(dFreeVec, c.norm) - c.contactDistance*invDt ); // dvfree = 1/dt * [ dot ( P - Q, n) - contactDist ] + dot(v_P - v_Q , n ) ] + + if (c.parameters.hasTangentialComponent()) + { + v->set(c.id + 1, dot(QP_vfree, c.t)); // dfree_t + v->set(c.id + 2, dot(QP_vfree, c.s)); // dfree_s + } + } +} + + +template +void BaseContactLagrangianConstraint::getConstraintViolation(const core::ConstraintParams *cparams, linearalgebra::BaseVector *v, const DataVecCoord &, const DataVecCoord & + , const DataVecDeriv &, const DataVecDeriv &) +{ + switch (cparams->constOrder()) + { + case core::ConstraintOrder::POS_AND_VEL : + case core::ConstraintOrder::POS : + getPositionViolation(v); + break; + + case core::ConstraintOrder::ACC : + case core::ConstraintOrder::VEL : + getVelocityViolation(v); + break; + + default : + msg_error() << "BaseContactLagrangianConstraint doesn't implement " << cparams->getName() << " constraint violation\n"; + break; + } +} + + +template +void BaseContactLagrangianConstraint::getConstraintInfo(const core::ConstraintParams*, VecConstraintBlockInfo& blocks, VecPersistentID& ids, VecConstCoord& /*positions*/, VecConstDeriv& directions, VecConstArea& /*areas*/) +{ + if (contacts.empty()) return; + const bool friction = (contacts[0].parameters.mu > 0.0); /// @todo: can there be both friction-less and friction contacts in the same BaseContactLagrangianConstraint ??? + ConstraintBlockInfo info; + info.parent = this; + info.const0 = contacts[0].id; + info.nbLines = friction ? 3 : 1; + info.hasId = true; + info.offsetId = ids.size(); + info.hasDirection = true; + info.offsetDirection = directions.size(); + info.nbGroups = contacts.size(); + + for (unsigned int i=0; i +bool BaseContactLagrangianConstraint::isActive() const +{ + for(unsigned int i = 0; i < contacts.size(); i++) + if(contacts[i].dfree < 0) + return true; + + return false; +} + +template +void BaseContactLagrangianConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowInteractionForceFields()) return; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + vparams->drawTool()->disableLighting(); + + std::vector redVertices; + std::vector otherVertices; + std::vector otherColors; + + for (unsigned int i=0; idrawTool()->drawLines(otherVertices, 3, otherColors); + + + + +} + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.cpp index ab1fa1724c8..97fbdedb43e 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.cpp @@ -20,6 +20,9 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #define SOFA_COMPONENT_CONSTRAINTSET_UNILATERALLAGRANGIANCONSTRAINT_CPP +#include +#include +#include #include #include #include @@ -28,6 +31,8 @@ namespace sofa::component::constraint::lagrangian::model { using namespace sofa::defaulttype; +using namespace sofa::helper; + void registerUnilateralLagrangianConstraint(sofa::core::ObjectFactory* factory) { @@ -35,6 +40,10 @@ void registerUnilateralLagrangianConstraint(sofa::core::ObjectFactory* factory) .add< UnilateralLagrangianConstraint >()); } +int UnilateralLagrangianConstraintClass = core::RegisterObject("UnilateralLagrangianConstraint") + .add< UnilateralLagrangianConstraint >(); + +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BaseContactLagrangianConstraint; template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralLagrangianConstraint; diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.h index f58a0ac1b72..8dd1f95f33c 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.h @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -34,124 +35,41 @@ namespace sofa::component::constraint::lagrangian::model { -template -class UnilateralLagrangianConstraint : public core::behavior::PairInteractionConstraint +struct UnilateralLagrangianContactParameters : public BaseContactParams { -public: - SOFA_CLASS(SOFA_TEMPLATE(UnilateralLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::PairInteractionConstraint,DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowConstIterator MatrixDerivRowConstIterator; - typedef typename DataTypes::MatrixDeriv::ColConstIterator MatrixDerivColConstIterator; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename DataTypes::MatrixDeriv::ColIterator MatrixDerivColIterator; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename Coord::value_type Real; - typedef typename core::behavior::MechanicalState MechanicalState; - - typedef core::behavior::BaseConstraint::ConstraintBlockInfo ConstraintBlockInfo; - typedef core::behavior::BaseConstraint::PersistentID PersistentID; - typedef core::behavior::BaseConstraint::ConstCoord ConstCoord; + UnilateralLagrangianContactParameters() : mu(0.0) {}; + UnilateralLagrangianContactParameters(SReal _mu) : mu(_mu) {}; - typedef core::behavior::BaseConstraint::VecConstraintBlockInfo VecConstraintBlockInfo; - typedef core::behavior::BaseConstraint::VecPersistentID VecPersistentID; - typedef core::behavior::BaseConstraint::VecConstCoord VecConstCoord; - typedef core::behavior::BaseConstraint::VecConstDeriv VecConstDeriv; - typedef core::behavior::BaseConstraint::VecConstArea VecConstArea; - - typedef core::objectmodel::Data DataVecCoord; - typedef core::objectmodel::Data DataVecDeriv; - typedef core::objectmodel::Data DataMatrixDeriv; - - typedef typename core::behavior::PairInteractionConstraint Inherit; - -protected: - - struct Contact + virtual bool hasTangentialComponent() const override { - int m1, m2; ///< the two extremities of the spring: masses m1 and m2 - Deriv norm; ///< contact normal, from m1 to m2 - Deriv t; ///< added for friction - Deriv s; ///< added for friction - Real contactDistance; - - unsigned int id; - long contactId; - PersistentID localId; - SReal mu; ///< angle for friction - - Coord P, Q; - - mutable Real dfree; - }; - - sofa::type::vector contacts; - Real epsilon; - bool yetIntegrated; - SReal customTolerance; - - PreviousForcesContainer prevForces; - bool* contactsStatus; - - /// Computes constraint violation in position and stores it into resolution global vector - /// - /// @param v Global resolution vector - virtual void getPositionViolation(linearalgebra::BaseVector *v); + return mu>0.0; + } - ///Computes constraint violation in velocity and stores it into resolution global vector - /// - /// @param v Global resolution vector - virtual void getVelocityViolation(linearalgebra::BaseVector *v); + SReal mu; +}; +template +class UnilateralLagrangianConstraint : public BaseContactLagrangianConstraint +{ public: + SOFA_CLASS(SOFA_TEMPLATE(UnilateralLagrangianConstraint,DataTypes), SOFA_TEMPLATE2(BaseContactLagrangianConstraint,DataTypes,UnilateralLagrangianContactParameters)); + typedef BaseContactLagrangianConstraint Inherit; + typedef typename Inherit::MechanicalState MechanicalState; + typedef typename Inherit::Contact Contact; - unsigned int constraintId; protected: - - virtual type::vector getUnilateralInteractionIdentifiers() {return {};} - - virtual type::vector getPairInteractionIdentifiers() override final - { - type::vector ids = getUnilateralInteractionIdentifiers(); - ids.push_back("Unilateral"); - return ids; - } - - UnilateralLagrangianConstraint(MechanicalState* object1=nullptr, MechanicalState* object2=nullptr); - virtual ~UnilateralLagrangianConstraint(); + virtual ~UnilateralLagrangianConstraint() = default; public: - void setCustomTolerance(SReal tol) { customTolerance = tol; } - - void clear(int reserve = 0); - - virtual void addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord Pfree, Coord Qfree, long id=0, PersistentID localid=0); - - void addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0); - void addContact(SReal mu, Deriv norm, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0); - - void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex - , const DataVecCoord &x1, const DataVecCoord &x2) override; - - void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord &x1, const DataVecCoord &x2 - , const DataVecDeriv &v1, const DataVecDeriv &v2) override; - - - void getConstraintInfo(const core::ConstraintParams* cParams, VecConstraintBlockInfo& blocks, VecPersistentID& ids, VecConstCoord& positions, VecConstDeriv& directions, VecConstArea& areas) override; - - void getConstraintResolution(const core::ConstraintParams *,std::vector& resTab, unsigned int& offset) override; - bool isActive() const override; + virtual void getConstraintResolution(const core::ConstraintParams *,std::vector& resTab, unsigned int& offset) override; - void draw(const core::visual::VisualParams* vparams) override; }; #if !defined(SOFA_COMPONENT_CONSTRAINTSET_UNILATERALLAGRANGIANCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralLagrangianConstraint; + extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BaseContactLagrangianConstraint; + extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralLagrangianConstraint; #endif diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.inl index 5112e0b46cc..f4b4d92ac6e 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.inl +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.inl @@ -32,340 +32,31 @@ namespace sofa::component::constraint::lagrangian::model template UnilateralLagrangianConstraint::UnilateralLagrangianConstraint(MechanicalState* object1, MechanicalState* object2) : Inherit(object1, object2) - , epsilon(Real(0.001)) - , yetIntegrated(false) - , customTolerance(0.0) - , contactsStatus(nullptr) { } -template -UnilateralLagrangianConstraint::~UnilateralLagrangianConstraint() -{ - if(contactsStatus) - delete[] contactsStatus; -} - -template -void UnilateralLagrangianConstraint::clear(int reserve) -{ - contacts.clear(); - if (reserve) - contacts.reserve(reserve); -} - -template -void UnilateralLagrangianConstraint::addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id, PersistentID localid) -{ - addContact(mu, norm, P, Q, contactDistance, m1, m2, - this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2], - this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1], - id, localid); -} - -template -void UnilateralLagrangianConstraint::addContact(SReal mu, Deriv norm, Real contactDistance, int m1, int m2, long id, PersistentID localid) -{ - addContact(mu, norm, - this->getMState2()->read(core::ConstVecCoordId::position())->getValue()[m2], - this->getMState1()->read(core::ConstVecCoordId::position())->getValue()[m1], - contactDistance, m1, m2, - this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2], - this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1], - id, localid); -} - -template -void UnilateralLagrangianConstraint::addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord /*Pfree*/, Coord /*Qfree*/, long id, PersistentID localid) -{ - contacts.resize(contacts.size() + 1); - Contact &c = contacts.back(); - - c.P = P; - c.Q = Q; - c.m1 = m1; - c.m2 = m2; - c.norm = norm; - c.t = Deriv(norm.z(), norm.x(), norm.y()); - c.s = cross(norm, c.t); - c.s = c.s / c.s.norm(); - c.t = cross((-norm), c.s); - c.mu = mu; - c.contactId = id; - c.localId = localid; - c.contactDistance = contactDistance; -} - - -template -void UnilateralLagrangianConstraint::buildConstraintMatrix(const core::ConstraintParams *, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &contactId - , const DataVecCoord &, const DataVecCoord &) -{ - assert(this->mstate1); - assert(this->mstate2); - - if (this->mstate1 == this->mstate2) - { - MatrixDeriv& c1 = *c1_d.beginEdit(); - - for (unsigned int i = 0; i < contacts.size(); i++) - { - Contact& c = contacts[i]; - - c.id = contactId++; - - MatrixDerivRowIterator c1_it = c1.writeLine(c.id); - - c1_it.addCol(c.m1, -c.norm); - c1_it.addCol(c.m2, c.norm); - - if (c.mu > 0.0) - { - c1_it = c1.writeLine(c.id + 1); - c1_it.setCol(c.m1, -c.t); - c1_it.setCol(c.m2, c.t); - - c1_it = c1.writeLine(c.id + 2); - c1_it.setCol(c.m1, -c.s); - c1_it.setCol(c.m2, c.s); - - contactId += 2; - } - } - - c1_d.endEdit(); - } - else - { - MatrixDeriv& c1 = *c1_d.beginEdit(); - MatrixDeriv& c2 = *c2_d.beginEdit(); - - for (unsigned int i = 0; i < contacts.size(); i++) - { - Contact& c = contacts[i]; - - c.id = contactId++; - - MatrixDerivRowIterator c1_it = c1.writeLine(c.id); - c1_it.addCol(c.m1, -c.norm); - - MatrixDerivRowIterator c2_it = c2.writeLine(c.id); - c2_it.addCol(c.m2, c.norm); - - if (c.mu > 0.0) - { - c1_it = c1.writeLine(c.id + 1); - c1_it.setCol(c.m1, -c.t); - - c1_it = c1.writeLine(c.id + 2); - c1_it.setCol(c.m1, -c.s); - - c2_it = c2.writeLine(c.id + 1); - c2_it.setCol(c.m2, c.t); - - c2_it = c2.writeLine(c.id + 2); - c2_it.setCol(c.m2, c.s); - - contactId += 2; - } - } - - c1_d.endEdit(); - c2_d.endEdit(); - } -} - - -template -void UnilateralLagrangianConstraint::getPositionViolation(linearalgebra::BaseVector *v) -{ - const VecCoord &PfreeVec = this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue(); - const VecCoord &QfreeVec = this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue(); - - Real dfree = (Real)0.0; - Real dfree_t = (Real)0.0; - Real dfree_s = (Real)0.0; - - const unsigned int cSize = contacts.size(); - - for (unsigned int i = 0; i < cSize; i++) - { - const Contact& c = contacts[i]; - - // Compute dfree, dfree_t and d_free_s - - const Coord &Pfree = PfreeVec[c.m2]; - const Coord &Qfree = QfreeVec[c.m1]; - - const Coord PPfree = Pfree - c.P; - const Coord QQfree = Qfree - c.Q; - - const Real ref_dist = PPfree.norm() + QQfree.norm(); - - dfree = dot(Pfree - Qfree, c.norm) - c.contactDistance; - const Real delta = dot(c.P - c.Q, c.norm) - c.contactDistance; - - if ((helper::rabs(delta) < 0.00001 * ref_dist) && (helper::rabs(dfree) < 0.00001 * ref_dist)) - { - dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); - dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); - } - else if (helper::rabs(delta - dfree) > 0.001 * delta) - { - const Real dt = delta / (delta - dfree); - - if (dt > 0.0 && dt < 1.0) - { - const Coord Pt = c.P * (1 - dt) + Pfree * dt; - const Coord Qt = c.Q * (1 - dt) + Qfree * dt; - const Coord PtPfree = Pfree - Pt; - const Coord QtQfree = Qfree - Qt; - - dfree_t = dot(PtPfree, c.t) - dot(QtQfree, c.t); - dfree_s = dot(PtPfree, c.s) - dot(QtQfree, c.s); - } - else if (dfree < 0.0) - { - dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); - dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); - } - else - { - dfree_t = 0; - dfree_s = 0; - } - } - else - { - dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); - dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); - } - - // Sets dfree in global violation vector - - v->set(c.id, dfree); - - c.dfree = dfree; // PJ : For isActive() method. Don't know if it's still useful. - - if (c.mu > 0.0) - { - v->set(c.id + 1, dfree_t); - v->set(c.id + 2, dfree_s); - } - } -} - - -template -void UnilateralLagrangianConstraint::getVelocityViolation(linearalgebra::BaseVector *v) -{ - auto P = this->getMState2()->readPositions(); - auto Q = this->getMState1()->readPositions(); - - const SReal dt = this->getContext()->getDt(); - const SReal invDt = SReal(1.0) / dt; - - const VecDeriv &PvfreeVec = this->getMState2()->read(core::ConstVecDerivId::freeVelocity())->getValue(); - const VecDeriv &QvfreeVec = this->getMState1()->read(core::ConstVecDerivId::freeVelocity())->getValue(); - - const unsigned int cSize = contacts.size(); - - for (unsigned int i = 0; i < cSize; i++) - { - const Contact& c = contacts[i]; - - const Deriv QP_invDt = (P[c.m2] - Q[c.m1])*invDt; - const Deriv QP_vfree = PvfreeVec[c.m2] - QvfreeVec[c.m1]; - const Deriv dFreeVec = QP_vfree + QP_invDt; - - v->set(c.id, dot(dFreeVec, c.norm) - c.contactDistance*invDt ); // dvfree = 1/dt * [ dot ( P - Q, n) - contactDist ] + dot(v_P - v_Q , n ) ] - - if (c.mu > 0.0) - { - v->set(c.id + 1, dot(QP_vfree, c.t)); // dfree_t - v->set(c.id + 2, dot(QP_vfree, c.s)); // dfree_s - } - } -} - - -template -void UnilateralLagrangianConstraint::getConstraintViolation(const core::ConstraintParams *cparams, linearalgebra::BaseVector *v, const DataVecCoord &, const DataVecCoord & - , const DataVecDeriv &, const DataVecDeriv &) -{ - switch (cparams->constOrder()) - { - case core::ConstraintOrder::POS_AND_VEL : - case core::ConstraintOrder::POS : - getPositionViolation(v); - break; - - case core::ConstraintOrder::ACC : - case core::ConstraintOrder::VEL : - getVelocityViolation(v); - break; - - default : - msg_error() << "UnilateralLagrangianConstraint doesn't implement " << cparams->getName() << " constraint violation\n"; - break; - } -} - - -template -void UnilateralLagrangianConstraint::getConstraintInfo(const core::ConstraintParams*, VecConstraintBlockInfo& blocks, VecPersistentID& ids, VecConstCoord& /*positions*/, VecConstDeriv& directions, VecConstArea& /*areas*/) -{ - if (contacts.empty()) return; - const bool friction = (contacts[0].mu > 0.0); /// @todo: can there be both friction-less and friction contacts in the same UnilateralLagrangianConstraint ??? - ConstraintBlockInfo info; - info.parent = this; - info.const0 = contacts[0].id; - info.nbLines = friction ? 3 : 1; - info.hasId = true; - info.offsetId = ids.size(); - info.hasDirection = true; - info.offsetDirection = directions.size(); - info.nbGroups = contacts.size(); - - for (unsigned int i=0; i void UnilateralLagrangianConstraint::getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) { - if(contactsStatus) + if(this->contactsStatus) { - delete[] contactsStatus; - contactsStatus = nullptr; + delete[] this->contactsStatus; + this->contactsStatus = nullptr; } - if (contacts.size() > 0) + if (this->contacts.size() > 0) { - contactsStatus = new bool[contacts.size()]; - memset(contactsStatus, 0, sizeof(bool)*contacts.size()); + this->contactsStatus = new bool[this->contacts.size()]; + memset(this->contactsStatus, 0, sizeof(bool)*this->contacts.size()); } - for(unsigned int i=0; icontacts.size(); i++) { - Contact& c = contacts[i]; - if(c.mu > 0.0) + Contact& c = this->contacts[i]; + if(c.parameters.hasTangentialComponent()) { - UnilateralConstraintResolutionWithFriction* ucrwf = new UnilateralConstraintResolutionWithFriction(c.mu, nullptr, &contactsStatus[i]); - ucrwf->setTolerance(customTolerance); + UnilateralConstraintResolutionWithFriction* ucrwf = new UnilateralConstraintResolutionWithFriction(c.parameters.mu, nullptr, &this->contactsStatus[i]); + ucrwf->setTolerance(this->customTolerance); resTab[offset] = ucrwf; // TODO : cette méthode de stockage des forces peu mal fonctionner avec 2 threads quand on utilise l'haptique @@ -374,48 +65,6 @@ void UnilateralLagrangianConstraint::getConstraintResolution(const co else resTab[offset++] = new UnilateralConstraintResolution(); } -} - -template -bool UnilateralLagrangianConstraint::isActive() const -{ - for(unsigned int i = 0; i < contacts.size(); i++) - if(contacts[i].dfree < 0) - return true; - - return false; -} - -template -void UnilateralLagrangianConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowInteractionForceFields()) return; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - vparams->drawTool()->disableLighting(); - - std::vector redVertices; - std::vector otherVertices; - std::vector otherColors; - - for (unsigned int i=0; idrawTool()->drawLines(otherVertices, 3, otherColors); - - - - } } //namespace sofa::component::constraint::lagrangian::model diff --git a/applications/plugins/CollisionOBBCapsule/src/CollisionOBBCapsule/response/contact/CapsuleContact.cpp b/applications/plugins/CollisionOBBCapsule/src/CollisionOBBCapsule/response/contact/CapsuleContact.cpp index e667cbfc1c5..29eb62b3706 100644 --- a/applications/plugins/CollisionOBBCapsule/src/CollisionOBBCapsule/response/contact/CapsuleContact.cpp +++ b/applications/plugins/CollisionOBBCapsule/src/CollisionOBBCapsule/response/contact/CapsuleContact.cpp @@ -27,6 +27,7 @@ #include #include #include +#include using namespace sofa::core::collision; @@ -79,6 +80,18 @@ template class COLLISIONOBBCAPSULE_API BarycentricPenalityContact, CapsuleCollisionModel>; template class COLLISIONOBBCAPSULE_API BarycentricPenalityContact, CylinderCollisionModel>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, CapsuleCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, TriangleCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, SphereCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, RigidSphereModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, CapsuleCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, CapsuleCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, TriangleCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, SphereCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, RigidSphereModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, OBBCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API BaseUnilateralContactResponse, OBBCollisionModel, sofa::component::constraint::lagrangian::model::UnilateralLagrangianContactParameters>; + template class COLLISIONOBBCAPSULE_API FrictionContact, CapsuleCollisionModel>; template class COLLISIONOBBCAPSULE_API FrictionContact, TriangleCollisionModel>; template class COLLISIONOBBCAPSULE_API FrictionContact, SphereCollisionModel>; diff --git a/applications/plugins/CollisionOBBCapsule/src/CollisionOBBCapsule/response/contact/OBBContact.cpp b/applications/plugins/CollisionOBBCapsule/src/CollisionOBBCapsule/response/contact/OBBContact.cpp index fe3f0a79efd..15fcd595c99 100644 --- a/applications/plugins/CollisionOBBCapsule/src/CollisionOBBCapsule/response/contact/OBBContact.cpp +++ b/applications/plugins/CollisionOBBCapsule/src/CollisionOBBCapsule/response/contact/OBBContact.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -57,6 +58,12 @@ template class COLLISIONOBBCAPSULE_API response::contact::BarycentricPenalityCon template class COLLISIONOBBCAPSULE_API response::contact::BarycentricPenalityContact, OBBCollisionModel>; template class COLLISIONOBBCAPSULE_API response::contact::BarycentricPenalityContact, OBBCollisionModel>; + +template class COLLISIONOBBCAPSULE_API response::contact::BaseUnilateralContactResponse, OBBCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API response::contact::BaseUnilateralContactResponse, OBBCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API response::contact::BaseUnilateralContactResponse, OBBCollisionModel,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; +template class COLLISIONOBBCAPSULE_API response::contact::BaseUnilateralContactResponse,constraint::lagrangian::model::UnilateralLagrangianContactParameters>; + template class COLLISIONOBBCAPSULE_API response::contact::FrictionContact, OBBCollisionModel>; template class COLLISIONOBBCAPSULE_API response::contact::FrictionContact, OBBCollisionModel>; template class COLLISIONOBBCAPSULE_API response::contact::FrictionContact, OBBCollisionModel>; diff --git a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.cpp b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.cpp index c6bf75f0684..6a0f20f31d5 100644 --- a/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.cpp +++ b/applications/plugins/SofaDistanceGrid/src/SofaDistanceGrid/RegisterModelToCollisionFactory.cpp @@ -35,6 +35,7 @@ using sofa::core::collision::Contact ; #include #include +#include #include "components/collision/DistanceGridCollisionModel.h" #include "components/collision/FFDDistanceGridDiscreteIntersection.h" diff --git a/examples/Demos/fallingBeamAugmentedLagrangianCollision.scn b/examples/Demos/fallingBeamAugmentedLagrangianCollision.scn new file mode 100644 index 00000000000..18f52f4a548 --- /dev/null +++ b/examples/Demos/fallingBeamAugmentedLagrangianCollision.scn @@ -0,0 +1,92 @@ + + + + + + + + + + + + + + + + + + + + + + + + + Container,TriangleSetTopologyModifier] --> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +