Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[Constraint.Lagrangian] Add augmented lagrangians #5003

Open
wants to merge 10 commits into
base: master
Choose a base branch
from
Open

[Constraint.Lagrangian] Add augmented lagrangians #5003

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
c48e9d9
Add AugmentedLagrangianConstraint response
bakpaul Aug 30, 2024
8f80ea4
accelerate GS convergence
bakpaul Aug 30, 2024
e0b41a5
Added virtualization layer for constraints, but linker issue arise. N…
bakpaul Sep 16, 2024
dc7f1d9
FIX linking
bakpaul Sep 17, 2024
14cb3a9
Add virtualization layer for contact response
bakpaul Sep 17, 2024
e7d04fb
Fix OBBCapsule
bakpaul Sep 18, 2024
1fd7dfc
Fix Distance grid and simplify fix for OBB
bakpaul Sep 18, 2024
1e0c177
Fix default friction used by tests
bakpaul Sep 24, 2024
129be9a
Try fix macos compilation
bakpaul Sep 30, 2024
9816f41
Merge remote-tracking branch 'origin/master' into test_augmented_lagr…
bakpaul Nov 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions Sofa/Component/Collision/Response/Contact/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down
85 changes: 85 additions & 0 deletions ...nse/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: [email protected] *
******************************************************************************/
#include <sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.inl>
#include <sofa/component/collision/response/contact/AugmentedLagrangianResponse.inl>
#include <sofa/component/collision/response/contact/BaseUnilateralContactResponse.inl>

#include <sofa/component/collision/response/mapper/RigidContactMapper.inl>
#include <sofa/component/collision/response/mapper/BarycentricContactMapper.inl>

namespace sofa::component::collision::response::contact
{

using namespace defaulttype;
using namespace sofa::helper;
using namespace sofa::component::collision::geometry;
using simulation::Node;

Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > PointPointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > LineSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > LinePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>> > LineLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > TrianglePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleTriangleAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > SphereSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > SpherePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<RigidSphereModel, RigidSphereModel> > RigidSphereRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > SphereRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > LineRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > TriangleRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>> > RigidSpherePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);

template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<RigidSphereModel, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;

template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<RigidSphereModel, RigidSphereModel>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>>;

} //namespace sofa::component::collision::response::contact
65 changes: 65 additions & 0 deletions ...ponse/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: [email protected] *
******************************************************************************/
#pragma once
#include <sofa/component/collision/response/contact/config.h>

#include <sofa/helper/Factory.h>
#include <sofa/core/collision/Contact.h>
#include <sofa/core/collision/Intersection.h>
#include <sofa/core/behavior/MechanicalState.h>
#include <sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.h>
#include <sofa/component/collision/response/contact/BaseUnilateralContactResponse.h>
#include <sofa/component/collision/response/contact/ContactIdentifier.h>

#include <sofa/core/objectmodel/RenamedData.h>

namespace sofa::component::collision::response::contact
{
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes = sofa::defaulttype::Vec3Types >
class AugmentedLagrangianResponse : public BaseUnilateralContactResponse<TCollisionModel1, TCollisionModel2,constraint::lagrangian::model::AugmentedLagrangianContactParameters, ResponseDataTypes>
{
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<DataTypes1> MechanicalState1;
typedef core::behavior::MechanicalState<DataTypes2> MechanicalState2;

Data<double> d_mu; ///< friction parameter
Data<double> 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
Loading
Loading