From a02af104f5b092effeb9ec81250100b210d11d92 Mon Sep 17 00:00:00 2001 From: Hugo Date: Thu, 9 Dec 2021 15:33:10 +0100 Subject: [PATCH] [plugins] Remove LMConstraint from SOFA repository (#2508) Prepare outsourcing of the LMConstraint plugin New plugin is now available at https://github.com/sofa-framework/LMConstraint --- applications/plugins/CMakeLists.txt | 1 - .../plugins/LMConstraint/CMakeLists.txt | 78 -- .../LMConstraint/LMConstraintConfig.cmake.in | 23 - .../examples/DOFBlockerLMConstraint.scn | 59 -- .../examples/DistanceLMConstraint.scn | 36 - .../examples/DistanceLMContactConstraint.scn | 49 - ...stanceLMContactConstraint_DirectSolver.scn | 63 -- .../examples/FixedLMConstraint.scn | 23 - .../LMConstraintCollisionResponse.scn | 58 -- ...BarycentricDistanceLMConstraintContact.cpp | 81 -- .../BarycentricDistanceLMConstraintContact.h | 103 -- ...BarycentricDistanceLMConstraintContact.inl | 232 ----- ...stanceLMConstraintContact_DistanceGrid.cpp | 56 -- .../src/LMConstraint/BaseLMConstraint.cpp | 112 --- .../src/LMConstraint/BaseLMConstraint.h | 246 ----- .../src/LMConstraint/ContactDescription.h | 66 -- .../LMConstraint/DOFBlockerLMConstraint.cpp | 46 - .../src/LMConstraint/DOFBlockerLMConstraint.h | 128 --- .../LMConstraint/DOFBlockerLMConstraint.inl | 186 ---- .../src/LMConstraint/DistanceLMConstraint.cpp | 80 -- .../src/LMConstraint/DistanceLMConstraint.h | 114 --- .../src/LMConstraint/DistanceLMConstraint.inl | 234 ----- .../DistanceLMContactConstraint.cpp | 46 - .../DistanceLMContactConstraint.h | 147 --- .../DistanceLMContactConstraint.inl | 380 ------- .../src/LMConstraint/FixedLMConstraint.cpp | 46 - .../src/LMConstraint/FixedLMConstraint.h | 117 --- .../src/LMConstraint/FixedLMConstraint.inl | 219 ---- .../src/LMConstraint/LMConstraint.cpp | 75 -- .../src/LMConstraint/LMConstraint.h | 189 ---- .../src/LMConstraint/LMConstraint.inl | 128 --- .../LMConstraint/LMConstraintDirectSolver.cpp | 311 ------ .../LMConstraint/LMConstraintDirectSolver.h | 58 -- .../src/LMConstraint/LMConstraintSolver.cpp | 946 ------------------ .../src/LMConstraint/LMConstraintSolver.h | 169 ---- .../MechanicalWriteLMConstraint.cpp | 60 -- .../MechanicalWriteLMConstraint.h | 82 -- .../PrecomputedLMConstraintCorrection.cpp | 45 - .../PrecomputedLMConstraintCorrection.h | 85 -- .../PrecomputedLMConstraintCorrection.inl | 259 ----- ...BarycentricDistanceLMConstraintContact.cpp | 49 - .../LMConstraint/src/LMConstraint/config.h.in | 34 - .../src/LMConstraint/initLMConstraint.cpp | 71 -- .../tutorials/houseOfCards/CMakeLists.txt | 9 - .../tutorials/houseOfCards/houseOfCards.cpp | 281 ------ 45 files changed, 5880 deletions(-) delete mode 100644 applications/plugins/LMConstraint/CMakeLists.txt delete mode 100644 applications/plugins/LMConstraint/LMConstraintConfig.cmake.in delete mode 100644 applications/plugins/LMConstraint/examples/DOFBlockerLMConstraint.scn delete mode 100644 applications/plugins/LMConstraint/examples/DistanceLMConstraint.scn delete mode 100644 applications/plugins/LMConstraint/examples/DistanceLMContactConstraint.scn delete mode 100644 applications/plugins/LMConstraint/examples/DistanceLMContactConstraint_DirectSolver.scn delete mode 100644 applications/plugins/LMConstraint/examples/FixedLMConstraint.scn delete mode 100644 applications/plugins/LMConstraint/examples/LMConstraintCollisionResponse.scn delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.inl delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact_DistanceGrid.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/BaseLMConstraint.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/BaseLMConstraint.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/ContactDescription.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.inl delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.inl delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.inl delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.inl delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.inl delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/LMConstraintDirectSolver.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/LMConstraintDirectSolver.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/LMConstraintSolver.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/LMConstraintSolver.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/MechanicalWriteLMConstraint.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/MechanicalWriteLMConstraint.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.h delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.inl delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/TetrahedronBarycentricDistanceLMConstraintContact.cpp delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/config.h.in delete mode 100644 applications/plugins/LMConstraint/src/LMConstraint/initLMConstraint.cpp delete mode 100644 applications/plugins/LMConstraint/tutorials/houseOfCards/CMakeLists.txt delete mode 100644 applications/plugins/LMConstraint/tutorials/houseOfCards/houseOfCards.cpp diff --git a/applications/plugins/CMakeLists.txt b/applications/plugins/CMakeLists.txt index e8c320fcc8b..95b05e67d8a 100644 --- a/applications/plugins/CMakeLists.txt +++ b/applications/plugins/CMakeLists.txt @@ -36,7 +36,6 @@ sofa_add_subdirectory_external(SofaPython3 SofaPython3) sofa_add_plugin_external(CGALPlugin CGALPlugin) # Depends on image sofa_add_plugin_external(Flexible Flexible) # Depends on image, CImgPlugin, SofaHighOrderTopology (soft) sofa_add_plugin(Registration Registration) # Depends on image, SofaPython, SofaSimpleGUI and #SOFADISTANCEGRID -sofa_add_plugin(LMConstraint LMConstraint ON) sofa_add_plugin(BulletCollisionDetection BulletCollisionDetection) # Depends on Compliant and LMConstraint sofa_add_plugin(PreassembledMass PreassembledMass) # Depends on Flexible and Compliant sofa_add_plugin(ExternalBehaviorModel ExternalBehaviorModel) diff --git a/applications/plugins/LMConstraint/CMakeLists.txt b/applications/plugins/LMConstraint/CMakeLists.txt deleted file mode 100644 index 0417b377d91..00000000000 --- a/applications/plugins/LMConstraint/CMakeLists.txt +++ /dev/null @@ -1,78 +0,0 @@ -cmake_minimum_required(VERSION 3.12) -project(LMConstraint VERSION 1.0 LANGUAGES CXX) - -sofa_find_package(SofaFramework REQUIRED) -sofa_find_package(SofaBase REQUIRED) -sofa_find_package(SofaImplicitOdeSolver REQUIRED) -sofa_find_package(SofaConstraint REQUIRED) -sofa_find_package(SofaMiscCollision REQUIRED) -sofa_find_package(SofaDistanceGrid QUIET) - -# List all files -set(LMCONSTRAINT_SRC_DIR src/LMConstraint) -set(HEADER_FILES - ${LMCONSTRAINT_SRC_DIR}/config.h.in - ${LMCONSTRAINT_SRC_DIR}/BaseLMConstraint.h - ${LMCONSTRAINT_SRC_DIR}/BarycentricDistanceLMConstraintContact.h - ${LMCONSTRAINT_SRC_DIR}/BarycentricDistanceLMConstraintContact.inl - ${LMCONSTRAINT_SRC_DIR}/LMConstraint.h - ${LMCONSTRAINT_SRC_DIR}/LMConstraint.inl - ${LMCONSTRAINT_SRC_DIR}/ContactDescription.h - ${LMCONSTRAINT_SRC_DIR}/DOFBlockerLMConstraint.h - ${LMCONSTRAINT_SRC_DIR}/DOFBlockerLMConstraint.inl - ${LMCONSTRAINT_SRC_DIR}/FixedLMConstraint.h - ${LMCONSTRAINT_SRC_DIR}/FixedLMConstraint.inl - ${LMCONSTRAINT_SRC_DIR}/DistanceLMContactConstraint.h - ${LMCONSTRAINT_SRC_DIR}/DistanceLMContactConstraint.inl - ${LMCONSTRAINT_SRC_DIR}/DistanceLMConstraint.h - ${LMCONSTRAINT_SRC_DIR}/DistanceLMConstraint.inl - ${LMCONSTRAINT_SRC_DIR}/LMConstraintSolver.h - ${LMCONSTRAINT_SRC_DIR}/LMConstraintDirectSolver.h - ${LMCONSTRAINT_SRC_DIR}/MechanicalWriteLMConstraint.h - ${LMCONSTRAINT_SRC_DIR}/PrecomputedLMConstraintCorrection.h - ${LMCONSTRAINT_SRC_DIR}/PrecomputedLMConstraintCorrection.inl - - ) - -set(SOURCE_FILES - ${LMCONSTRAINT_SRC_DIR}/initLMConstraint.cpp - ${LMCONSTRAINT_SRC_DIR}/BaseLMConstraint.cpp - ${LMCONSTRAINT_SRC_DIR}/BarycentricDistanceLMConstraintContact.cpp - ${LMCONSTRAINT_SRC_DIR}/LMConstraint.cpp - ${LMCONSTRAINT_SRC_DIR}/DOFBlockerLMConstraint.cpp - ${LMCONSTRAINT_SRC_DIR}/FixedLMConstraint.cpp - ${LMCONSTRAINT_SRC_DIR}/DistanceLMContactConstraint.cpp - ${LMCONSTRAINT_SRC_DIR}/DistanceLMConstraint.cpp - ${LMCONSTRAINT_SRC_DIR}/LMConstraintSolver.cpp - ${LMCONSTRAINT_SRC_DIR}/LMConstraintDirectSolver.cpp - ${LMCONSTRAINT_SRC_DIR}/MechanicalWriteLMConstraint.cpp - ${LMCONSTRAINT_SRC_DIR}/PrecomputedLMConstraintCorrection.cpp - ${LMCONSTRAINT_SRC_DIR}/TetrahedronBarycentricDistanceLMConstraintContact.cpp - ) - -if(SofaDistanceGrid_FOUND) - list(APPEND SOURCE_FILES - ${LMCONSTRAINT_SRC_DIR}/BarycentricDistanceLMConstraintContact_DistanceGrid.cpp - ) -else() - message(STATUS "SofaDistanceGrid not found: DistanceLM codes will not be compiled") -endif() - - -# Create the plugin library -add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES} ) -target_link_libraries(${PROJECT_NAME} PUBLIC SofaCore SofaBaseLinearSolver SofaImplicitOdeSolver SofaHelper SofaDefaultType SofaConstraint SofaMiscCollision) - -if(SofaDistanceGrid_FOUND) - target_link_libraries(${PROJECT_NAME} PUBLIC SofaDistanceGrid) -endif() - -# Create package Config, Version & Target files. -sofa_create_package_with_targets( - PACKAGE_NAME ${PROJECT_NAME} - PACKAGE_VERSION ${PROJECT_VERSION} - TARGETS ${PROJECT_NAME} AUTO_SET_TARGET_PROPERTIES - INCLUDE_SOURCE_DIR "src" - INCLUDE_INSTALL_DIR ${PROJECT_NAME} - RELOCATABLE "plugins" - ) diff --git a/applications/plugins/LMConstraint/LMConstraintConfig.cmake.in b/applications/plugins/LMConstraint/LMConstraintConfig.cmake.in deleted file mode 100644 index 3d69ad9464c..00000000000 --- a/applications/plugins/LMConstraint/LMConstraintConfig.cmake.in +++ /dev/null @@ -1,23 +0,0 @@ -# CMake package configuration file for the plugin @PROJECT_NAME@ - -@PACKAGE_GUARD@ -@PACKAGE_INIT@ - -set(LMCONSTRAINT_HAVE_SOFADISTANCEGRID @LMCONSTRAINT_HAVE_SOFADISTANCEGRID@) - -find_package(SofaFramework REQUIRED) -find_package(SofaBase REQUIRED) -find_package(SofaImplicitOdeSolver REQUIRED) -find_package(SofaConstraint REQUIRED) - - -if(LMCONSTRAINT_HAVE_SOFADISTANCEGRID) - find_package(SofaDistanceGrid REQUIRED) -endif() - - -if(NOT TARGET @PROJECT_NAME@) - include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") -endif() - -check_required_components(@PROJECT_NAME@) diff --git a/applications/plugins/LMConstraint/examples/DOFBlockerLMConstraint.scn b/applications/plugins/LMConstraint/examples/DOFBlockerLMConstraint.scn deleted file mode 100644 index 8eeae97011a..00000000000 --- a/applications/plugins/LMConstraint/examples/DOFBlockerLMConstraint.scn +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/applications/plugins/LMConstraint/examples/DistanceLMConstraint.scn b/applications/plugins/LMConstraint/examples/DistanceLMConstraint.scn deleted file mode 100644 index 2cf5cf7502a..00000000000 --- a/applications/plugins/LMConstraint/examples/DistanceLMConstraint.scn +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/LMConstraint/examples/DistanceLMContactConstraint.scn b/applications/plugins/LMConstraint/examples/DistanceLMContactConstraint.scn deleted file mode 100644 index 14ee1b28e4a..00000000000 --- a/applications/plugins/LMConstraint/examples/DistanceLMContactConstraint.scn +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/LMConstraint/examples/DistanceLMContactConstraint_DirectSolver.scn b/applications/plugins/LMConstraint/examples/DistanceLMContactConstraint_DirectSolver.scn deleted file mode 100644 index cbf1a1588b1..00000000000 --- a/applications/plugins/LMConstraint/examples/DistanceLMContactConstraint_DirectSolver.scn +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/LMConstraint/examples/FixedLMConstraint.scn b/applications/plugins/LMConstraint/examples/FixedLMConstraint.scn deleted file mode 100644 index 76037cba503..00000000000 --- a/applications/plugins/LMConstraint/examples/FixedLMConstraint.scn +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/LMConstraint/examples/LMConstraintCollisionResponse.scn b/applications/plugins/LMConstraint/examples/LMConstraintCollisionResponse.scn deleted file mode 100644 index e0839adc793..00000000000 --- a/applications/plugins/LMConstraint/examples/LMConstraintCollisionResponse.scn +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.cpp b/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.cpp deleted file mode 100644 index 402ac012f2b..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/****************************************************************************** -* 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 -#include - -using namespace sofa::defaulttype; -using namespace sofa::core::collision; - -namespace sofa::component::collision -{ - -using simulation::Node; - -Creator, SphereCollisionModel> > SphereSphereDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, PointCollisionModel> > SpherePointDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, PointCollisionModel> > PointPointDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, PointCollisionModel> > LinePointDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, LineCollisionModel> > LineLineDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, SphereCollisionModel> > LineSphereDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, SphereCollisionModel> > TriangleSphereDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, PointCollisionModel> > TrianglePointDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, LineCollisionModel> > TriangleLineDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator, TriangleCollisionModel> > TriangleTriangleDistanceLMConstraintContactClass("distanceLMConstraint",true); - -Creator, RigidSphereModel> > TriangleRigidSphereLMConstraintContactClass("distanceLMConstraint",true); -Creator, PointCollisionModel> > TrianglePointLMConstraintContactClass("distanceLMConstraint",true); -Creator, LineCollisionModel> > TriangleLineLMConstraintContactClass("distanceLMConstraint",true); -Creator, TriangleCollisionModel> > TriangleTriangleLMConstraintContactClass("distanceLMConstraint",true); -Creator, TriangleCollisionModel> > CapsuleTriangleLMConstraintContactClass("distanceLMConstraint",true); -Creator, LineCollisionModel> > CapsuleLineLMConstraintContactClass("distanceLMConstraint",true); -Creator, CapsuleCollisionModel> > CapsuleCapsuleLMConstraintContactClass("distanceLMConstraint",true); -Creator, SphereCollisionModel> > CapsuleSphereLMConstraintContactClass("distanceLMConstraint",true); -Creator, RigidSphereModel> > CapsuleRigidSphereLMConstraintContactClass("distanceLMConstraint",true); -Creator, OBBCollisionModel> > OBBOBBLMConstraintContactClass("distanceLMConstraint",true); -Creator, OBBCollisionModel> > CapsuleOBBLMConstraintContactClass("distanceLMConstraint",true); -Creator, OBBCollisionModel> > SphereOBBLMConstraintContactClass("distanceLMConstraint",true); -Creator> > RigidSphereOBBLMConstraintContactClass("distanceLMConstraint",true); -Creator, OBBCollisionModel> > TriangleOBBLMConstraintContactClass("distanceLMConstraint",true); - - -Creator, TriangleCollisionModel> > RigidCapsuleTriangleLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, CapsuleCollisionModel> > RigidCapsuleRigidCapsuleLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, CapsuleCollisionModel> > CapsuleRigidCapsuleLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, SphereCollisionModel> > RigidCapsuleSphereLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, RigidSphereModel> > RigidCapsuleRigidSphereLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, OBBCollisionModel> > RigidCapsuleOBBLMConstraintContactClassClass("distanceLMConstraint",true); - - -Creator, CylinderCollisionModel> > CylinderCylinderLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, TriangleCollisionModel> > CylinderTriangleLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, CapsuleCollisionModel> > CylinderRigidCapsuleLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, CylinderCollisionModel> > CapsuleCylinderLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, SphereCollisionModel> > CylinderSphereLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, RigidSphereModel> > CylinderRigidSphereLMConstraintContactClassClass("distanceLMConstraint",true); -Creator, OBBCollisionModel> > CylinderOBBLMConstraintContactClassClass("distanceLMConstraint",true); - -} // namespace sofa::component::collision diff --git a/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.h b/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.h deleted file mode 100644 index 756199b65fc..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.h +++ /dev/null @@ -1,103 +0,0 @@ -/****************************************************************************** -* 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 - -#ifdef __APPLE__ -// a strange behaviour of the mac's linker requires to compile a few stuffs again -#include -#include -#endif - - -namespace sofa::component::collision -{ - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes = sofa::defaulttype::Vec3Types > -class BarycentricDistanceLMConstraintContact : public core::collision::Contact -{ -public: - SOFA_CLASS(SOFA_TEMPLATE3(BarycentricDistanceLMConstraintContact, TCollisionModel1, TCollisionModel2, ResponseDataTypes), core::collision::Contact); - typedef TCollisionModel1 CollisionModel1; - typedef TCollisionModel2 CollisionModel2; - typedef core::collision::Intersection Intersection; - typedef core::collision::DetectionOutputVector OutputVector; - typedef core::collision::TDetectionOutputVector TOutputVector; - typedef ResponseDataTypes DataTypes1; - typedef ResponseDataTypes DataTypes2; - typedef core::behavior::MechanicalState MechanicalState1; - typedef core::behavior::MechanicalState MechanicalState2; - typedef typename CollisionModel1::Element CollisionElement1; - typedef typename CollisionModel2::Element CollisionElement2; - - // response type - typedef constraintset::DistanceLMContactConstraint ResponseType; - -protected: - CollisionModel1* model1; - CollisionModel2* model2; - Intersection* intersectionMethod; - - ContactMapper mapper1; - ContactMapper mapper2; - - typename ResponseType::SPtr ff; - core::objectmodel::BaseContext* parent; - - typedef std::map ContactIndexMap; - /// Mapping of contactids to force element (+1, so that 0 means not active). - /// This allows to ignore duplicate contacts, and preserve information associated with each contact point over time - ContactIndexMap contactIndex; - - BarycentricDistanceLMConstraintContact(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod); - ~BarycentricDistanceLMConstraintContact() override; - - void setInteractionTags(MechanicalState1* mstate1, MechanicalState2* mstate2); - -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; - - void draw(const core::visual::VisualParams* vparams) override; -}; - -} // namespace sofa::component::collision diff --git a/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.inl b/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.inl deleted file mode 100644 index ab8a850f6b1..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact.inl +++ /dev/null @@ -1,232 +0,0 @@ -/****************************************************************************** -* 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 - -namespace sofa::component::collision -{ - - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -BarycentricDistanceLMConstraintContact::BarycentricDistanceLMConstraintContact(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod) - : model1(model1), model2(model2), intersectionMethod(intersectionMethod), ff(nullptr), parent(nullptr) -{ - mapper1.setCollisionModel(model1); - mapper2.setCollisionModel(model2); -} - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -BarycentricDistanceLMConstraintContact::~BarycentricDistanceLMConstraintContact() -{ -} - - - - - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void BarycentricDistanceLMConstraintContact::cleanup() -{ - if (ff!=nullptr) - { - ff->cleanup(); - if (parent!=nullptr) parent->removeObject(ff); - parent = nullptr; - ff.reset(); - mapper1.cleanup(); - mapper2.cleanup(); - } -} - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void BarycentricDistanceLMConstraintContact::setDetectionOutputs(OutputVector* o) -{ - TOutputVector& outputs = *static_cast(o); - if (ff==nullptr) - { - - MechanicalState1* mstate1 = mapper1.createMapping(GenerateStringID::generate().c_str()); - MechanicalState2* mstate2 = mapper2.createMapping(GenerateStringID::generate().c_str()); - ff = sofa::core::objectmodel::New(mstate1,mstate2); - ff->setName( getName() ); - setInteractionTags(mstate1, mstate2); - ff->init(); - } - - int insize = outputs.size(); - - // old index for each contact - // >0 indicate preexisting contact - // 0 indicate new contact - // -1 indicate ignored duplicate contact - std::vector oldIndex(insize); - - int nbnew = 0; - - for (int i=0; iid]; - if (index < 0) // duplicate contact - { - int i2 = -1-index; - core::collision::DetectionOutput* o2 = &outputs[i2]; - if (o2->value <= o->value) - { - // current contact is ignored - oldIndex[i] = -1; - continue; - } - else - { - // previous contact is replaced - oldIndex[i] = oldIndex[i2]; - oldIndex[i2] = -1; - } - } - else - { - oldIndex[i] = index; - if (!index) - { - ++nbnew; - msg_info() << " new contact "<id ; - } - } - index = -1-i; // save this index as a negative value in contactIndex map. - } - - // compute new index of each contact - std::vector newIndex(insize); - // number of final contacts used in the response - int size = 0; - for (int i=0; i= 0) - { - ++size; - newIndex[i] = size; - } - } - - // update contactMap - for (ContactIndexMap::iterator it = contactIndex.begin(), itend = contactIndex.end(); it != itend; ) - { - int& index = it->second; - if (index >= 0) - { - msg_info() << " removed contact "<first; - ContactIndexMap::iterator oldit = it; - ++it; - contactIndex.erase(oldit); - } - else - { - index = newIndex[-1-index]; // write the final contact index - ++it; - } - } - msg_info() << " "<clear(); - mapper1.resize(size); - mapper2.resize(size); - for (int i=0; ielem.first); - CollisionElement2 elem2(o->elem.second); - int index1 = elem1.getIndex(); - int index2 = elem2.getIndex(); - typename DataTypes1::Real r1 = 0.0; - typename DataTypes2::Real r2 = 0.0; - // Create mapping for first point - index1 = mapper1.addPointB(o->point[0], index1, r1); - // Create mapping for second point - index2 = mapper2.addPointB(o->point[1], index2, r2); - - ff->addContact(index1,index2); - ff->contactFriction.setValue( elem1.getCollisionModel()->getContactFriction(0) * elem2.getCollisionModel()->getContactFriction(0) ); - // if all the elements belong to the same CollisionModels then ff->contactFriction should be done only once... - } - // Update mappings - mapper1.update(); - mapper2.update(); -} - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void BarycentricDistanceLMConstraintContact::createResponse(core::objectmodel::BaseContext* group) -{ - if (ff!=nullptr) - { - if (parent!=nullptr) - { - parent->removeObject(this); - parent->removeObject(ff); - } - parent = group; - if (parent!=nullptr) - { - //msg_error() << "Attaching contact response to "<getName() ; - parent->addObject(this); - parent->addObject(ff); - } - } -} - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void BarycentricDistanceLMConstraintContact::removeResponse() -{ - if (ff!=nullptr) - { - if (parent!=nullptr) - { - parent->removeObject(this); - parent->removeObject(ff); - } - parent = nullptr; - } -} - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void BarycentricDistanceLMConstraintContact::draw(const core::visual::VisualParams* ) -{ -} - -template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes > -void BarycentricDistanceLMConstraintContact::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) - ff->addTag(*it); - for(it=tagsm2.begin(); it!=tagsm2.end(); ++it) - ff->addTag(*it); -} - - -} // namespace sofa::component::collision diff --git a/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact_DistanceGrid.cpp b/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact_DistanceGrid.cpp deleted file mode 100644 index 2a3ed0311ea..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/BarycentricDistanceLMConstraintContact_DistanceGrid.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/****************************************************************************** -* 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 - - -using namespace sofa::defaulttype; -using namespace sofa::core::collision; - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -Creator > DistanceGridDistanceGridDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > DistanceGridPointDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > DistanceGridSphereDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > DistanceGridTriangleDistanceLMConstraintContactClass("distanceLMConstraint",true); - - -Creator > FFDDistanceGridDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator > FFDDistanceGridRigidDistanceGridDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > FFDDistanceGridPointDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > FFDDistanceGridSphereDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > FFDDistanceGridTriangleDistanceLMConstraintContactClass("distanceLMConstraint",true); - -} // namespace collision - -} // namespace component - -} // namespace sofa - diff --git a/applications/plugins/LMConstraint/src/LMConstraint/BaseLMConstraint.cpp b/applications/plugins/LMConstraint/src/LMConstraint/BaseLMConstraint.cpp deleted file mode 100644 index a3b060ca843..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/BaseLMConstraint.cpp +++ /dev/null @@ -1,112 +0,0 @@ -/****************************************************************************** -* 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 -namespace sofa -{ - -namespace core -{ - -namespace behavior -{ -//------------------------------------------------------------------------ -//ConstraintGroup - -ConstraintGroup::ConstraintGroup(ConstraintParams::ConstOrder idConstraint) - : Order(idConstraint) - , active(true) -{ -} - -void ConstraintGroup::addConstraint( unsigned int &constraintId, unsigned int idx, SReal c) -{ - equations.resize(equations.size()+1); - ConstraintEquation &eq=equations.back(); - - eq.idx = idx; - eq.correction=c; - - eq.constraintId = constraintId; - constraintId++; -} -//------------------------------------------------------------------------ -BaseLMConstraint::BaseLMConstraint() - : pathObject1( initData(&pathObject1, "object1","First Object to constrain") ), - pathObject2( initData(&pathObject2, "object2","Second Object to constrain") ) -{ -} - -unsigned int BaseLMConstraint::getNumConstraint(ConstraintParams::ConstOrder Order) -{ - size_t result=0; - const type::vector< ConstraintGroup* > &vec = constraintOrder[Order]; - for (size_t i=0; igetNumConstraint(); - return static_cast(result); -} - -ConstraintGroup* BaseLMConstraint::addGroupConstraint(ConstraintParams::ConstOrder id) -{ - ConstraintGroup *c = new ConstraintGroup(id); - constraintOrder[id].push_back(c); - return c; -} - -void BaseLMConstraint::getConstraintViolation(const core::ConstraintParams* cparams, defaulttype::BaseVector *v) -{ - getConstraintViolation(v,cparams->constOrder()); -} - -void BaseLMConstraint::getConstraintViolation(defaulttype::BaseVector * v, const sofa::core::ConstraintParams::ConstOrder Order) -{ - const type::vector< ConstraintGroup* > &constraints = constraintOrder[Order]; - for (size_t idxGroupConstraint=0; idxGroupConstraint range=group->data(); - - for (ConstraintGroup::EquationIterator equation = range.first; equation != range.second; ++equation) - { - v->set(equation->constraintId, equation->correction); - } - } - -} - -void BaseLMConstraint::resetConstraint() -{ - std::map< ConstraintParams::ConstOrder, type::vector< ConstraintGroup*> >::iterator it; - for (it=constraintOrder.begin(); it!=constraintOrder.end(); ++it) - { - type::vector< ConstraintGroup* > &v=it->second; - for (size_t i=0; i. * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_CONSTRAINT_BASELMCONSTRAINT_H -#define SOFA_COMPONENT_CONSTRAINT_BASELMCONSTRAINT_H - -#include -#include -#include - -namespace sofa -{ - -namespace core -{ - -namespace behavior -{ - - - - -/** - * \brief Expression of a line of the system created to solve the constraint -* -* @param idx index of the equation in the constraint equation system stored within the mechanical states -* @param correction right hand term of the equation: corresponds to a correction we have to apply to the system -* @param constraintId actual index of the line corresponding to the constraint equation in the whole system: can be different from idx -**/ -struct LMCONSTRAINT_API ConstraintEquation -{ - int idx; - SReal correction; - unsigned int constraintId; -}; - - - - - -/** - * \brief Intern storage of the constraints. - * a ConstraintGroup is a list of equations that will be solved together. - * They are defined by a ConstOrder(position, velocity or acceleration) - * @see ConstraintEquation - * @see ConstOrder - **/ -class LMCONSTRAINT_API ConstraintGroup -{ - typedef sofa::type::vector< ConstraintEquation > VecEquations; -public: - typedef VecEquations::const_iterator EquationConstIterator; - typedef VecEquations::iterator EquationIterator; - - ConstraintGroup(ConstraintParams::ConstOrder idConstraint); - /** - * Method to add an interaction constraint to the group - * - * @param idx index of the equation - * @param c correction we need to apply in order to solve the constraint - **/ - void addConstraint( unsigned int &constraintId, unsigned int idx, SReal c); - - - - /// Random Access to an equation - const ConstraintEquation &getConstraint(const unsigned int i) const - { - EquationConstIterator it=equations.begin(); - std::advance(it,i); - return *it; - } - - ConstraintEquation &getConstraint(const unsigned int i) - { - EquationIterator it=equations.begin(); - std::advance(it,i); - return *it; - } - - - /// Retrieve all the equations - std::pair< EquationConstIterator,EquationConstIterator> data() const - { - return std::make_pair( equations.begin(), equations.end()); - } - - std::pair< EquationIterator,EquationIterator > data() - { - return std::make_pair( equations.begin(), equations.end()); - } - - - /// Return the number of constraint contained in this group - std::size_t getNumConstraint() const { return equations.size();} - - /// Return the order of the constraint - /// @see ConstOrder - ConstraintParams::ConstOrder getOrder() const { return Order;} - - bool isActive()const {return active;} - void setActive(bool b) {active=b;} -protected: - /// Order of the constraint - /// @see ConstOrder - ConstraintParams::ConstOrder Order; - VecEquations equations; - bool active; -}; - - -/** - * \brief Object storing constraints base on Lagrangrian Multipliers. - * - * They can be constraint on acceleration, velocity, or position. - * They can be grouped or individual. The resolution is then done in the OdeSolver. - **/ -class LMCONSTRAINT_API BaseLMConstraint: public BaseConstraintSet -{ -public: - SOFA_ABSTRACT_CLASS(BaseLMConstraint, BaseConstraintSet); - -protected: - BaseLMConstraint(); - ~BaseLMConstraint() override {} - -public: - - /// Called by MechanicalWriteLMConstaint: The Object will compute the constraints present in the current state, and create the ConstraintGroup related. - virtual void writeConstraintEquations(unsigned int& lineNumber, MultiVecId id, ConstraintParams::ConstOrder order)=0; - - /// Compute the new Lagrange Multiplier given a block of the compliance matrix W, and the current correction (left hand term) and previous Lagrange Multiplier - virtual void LagrangeMultiplierEvaluation(const SReal* /*W*/, - const SReal* /*c*/, SReal* /*Lambda*/, - ConstraintGroup * /*group*/) {} - - - /// Get Right Hand Term - void getConstraintViolation(const sofa::core::ConstraintParams*, defaulttype::BaseVector * /*v*/ ) override; - - using BaseConstraintSet::getConstraintViolation; - // Override used in LMConstraintSolver::buildSystem method - void getConstraintViolation(defaulttype::BaseVector *v, const core::ConstraintParams::ConstOrder ); - - - /// Get the internal structure: return all the constraint stored by their nature in a map - virtual void getConstraints( std::map< ConstraintParams::ConstOrder, type::vector< ConstraintGroup* > > &i) { i=constraintOrder;} - /// Get all the constraints stored of a given nature - virtual const type::vector< ConstraintGroup* > &getConstraintsOrder(ConstraintParams::ConstOrder Order) const - { - constraintOrder_t::const_iterator c = constraintOrder.find( Order ); - if( c == constraintOrder.end()) - { - static type::vector emptyVector; - return emptyVector; - } - return c->second; - } - - - /// Get Left Hand Term for a given constraint group - template - void getEquationsUsed(const ConstraintGroup* group, DataStorage &used0) const - { - typedef ConstraintGroup::EquationConstIterator iterator_t; - std::pair< iterator_t, iterator_t > range=group->data(); - for (iterator_t equation=range.first; equation!=range.second; ++equation) used0.push_back(equation->idx); - } - - /// Get Left Hand Term for each ConstraintGroup of a given order - template - void getEquationsUsed(ConstraintParams::ConstOrder Order, DataStorage &used0) const - { - constraintOrder_t::const_iterator g = constraintOrder.find(Order); - if (g == constraintOrder.end()) return; - - const type::vector< ConstraintGroup* > &constraints = g->second; - for (unsigned int idxGroupConstraint=0; idxGroupConstraint > constraintOrder_t; - constraintOrder_t constraintOrder; - - Data pathObject1; ///< First Object to constrain - Data pathObject2; ///< Second Object to constrain -}; - -} // namespace behavior - -} // namespace core - -} // namespace sofa - - -#endif diff --git a/applications/plugins/LMConstraint/src/LMConstraint/ContactDescription.h b/applications/plugins/LMConstraint/src/LMConstraint/ContactDescription.h deleted file mode 100644 index ab964d4c668..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/ContactDescription.h +++ /dev/null @@ -1,66 +0,0 @@ -/****************************************************************************** -* 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 - -namespace sofa::component::constraintset -{ - -enum ContactState {VANISHING, STICKING, SLIDING}; - -/// ContactDescription is a class providing precise information about the state of a contact -/// A contact can be in 3 different states: vanishing, sticking, sliding -/// with this states, we can solve precisely a contact, knowning its state, and the direction of the response -struct ContactDescription -{ - /// State of the contact - /// @see ContactState - ContactState state; - /// When the contact is sliding, we have to know the direction of the reponse force. - /// coeff is the linear combination of the normal, and two tangent constraint directions. - SReal coeff[3]; -}; - - -/// Class handler to make the kink between constraint groups (a set of equations related to contact) and a description of the state of the contact -class ContactDescriptionHandler -{ - typedef std::map< const core::behavior::ConstraintGroup*, ContactDescription> InternalData; -public: - const ContactDescription& getContactDescription( const core::behavior::ConstraintGroup* contact) const - { - InternalData::const_iterator it = infos.find(contact); - assert (it != infos.end()); - return it->second; - } - ContactDescription& getContactDescription(const core::behavior::ConstraintGroup* contact) - { - return infos[contact]; - } -protected: - InternalData infos; -}; - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.cpp b/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.cpp deleted file mode 100644 index becc99f5e05..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/****************************************************************************** -* 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_DOFBLOCKERLMCONSTRAINT_CPP -#include -#include -#include -#include - -namespace sofa::component::constraintset -{ - -using namespace sofa::defaulttype; -using namespace sofa::helper; - -int DOFBlockerLMConstraintClass = core::RegisterObject("Constrain the rotation of a given set of Rigid Bodies") - .add< DOFBlockerLMConstraint >() - .add< DOFBlockerLMConstraint >() - - ; - -template class DOFBlockerLMConstraint; -template class DOFBlockerLMConstraint; - - - - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.h b/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.h deleted file mode 100644 index 7e495d672b2..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.h +++ /dev/null @@ -1,128 +0,0 @@ -/****************************************************************************** -* 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::constraintset -{ - - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class DOFBlockerLMConstraintInternalData -{ -}; - - - - -/** Keep two particules at an initial distance -*/ -template -class DOFBlockerLMConstraint : public core::behavior::LMConstraint -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(DOFBlockerLMConstraint,DataTypes),SOFA_TEMPLATE2(sofa::core::behavior::LMConstraint, DataTypes, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename core::behavior::MechanicalState MechanicalState; - - - typedef sofa::component::topology::TopologySubsetIndices SetIndex; - typedef type::vector SetIndexArray; - - typedef core::ConstraintParams::ConstOrder ConstOrder; - - -protected: - DOFBlockerLMConstraintInternalData data; - friend class DOFBlockerLMConstraintInternalData; - - - DOFBlockerLMConstraint(MechanicalState *dof = nullptr) - : core::behavior::LMConstraint(dof, dof) - , BlockedAxis(core::objectmodel::Base::initData(&BlockedAxis, "rotationAxis", "List of rotation axis to constrain")) - , factorAxis(core::objectmodel::Base::initData(&factorAxis, "factorAxis", "Factor to apply in order to block only a certain amount of rotation along the axis")) - , f_indices(core::objectmodel::Base::initData(&f_indices, "indices", "List of the index of particles to be fixed")) - , showSizeAxis(core::objectmodel::Base::initData(&showSizeAxis, 1.0f, "showSizeAxis", "size of the vector used to display the constrained axis")) - , l_topology(initLink("topology", "link to the topology container")) - { - - } - - ~DOFBlockerLMConstraint() - { - - } - -public: - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - - void init() override; - void draw(const core::visual::VisualParams* vparams) override; - void resetConstraint() override; - - // -- LMConstraint interface - void buildConstraintMatrix(const core::ConstraintParams* cParams, core::MultiMatrixDerivId cId, unsigned int &cIndex) override; - void writeConstraintEquations(unsigned int& lineNumber, core::MultiVecId id, ConstOrder order) override; - - bool isCorrectionComputedWithSimulatedDOF(ConstOrder /*order*/) const override - { - simulation::Node* node=(simulation::Node*) this->constrainedObject1->getContext(); - if (node->mechanicalMapping.empty()) return true; - else return false; - } - - Data > BlockedAxis; ///< List of rotation axis to constrain - Data > factorAxis; ///< Factor to apply in order to block only a certain amount of rotation along the axis - SetIndex f_indices; ///< List of the index of particles to be fixed - Data showSizeAxis; ///< size of the vector used to display the constrained axis - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected : - sofa::type::vector idxEquations; -}; - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_DOFBLOCKERLMCONSTRAINT_CPP) -extern template class DOFBlockerLMConstraint; -extern template class DOFBlockerLMConstraint; - -#endif - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.inl b/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.inl deleted file mode 100644 index 59976e2a24c..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DOFBlockerLMConstraint.inl +++ /dev/null @@ -1,186 +0,0 @@ -/****************************************************************************** -* 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 - - -namespace sofa::component::constraintset -{ - -template -void DOFBlockerLMConstraint::clearConstraints() -{ - SetIndexArray& _indices = *f_indices.beginEdit(); - _indices.clear(); - f_indices.endEdit(); -} - -template -void DOFBlockerLMConstraint::addConstraint(Index index) -{ - f_indices.beginEdit()->push_back(index); - f_indices.endEdit(); -} - -template -void DOFBlockerLMConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*f_indices.beginEdit(),index); - f_indices.endEdit(); -} - - -template -void DOFBlockerLMConstraint::init() -{ - core::behavior::LMConstraint::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - sofa::core::topology::BaseMeshTopology* _topology = l_topology.get(); - - if (_topology) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological change handling - f_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } -} - - -template -void DOFBlockerLMConstraint::resetConstraint() -{ - core::behavior::LMConstraint::resetConstraint(); - idxEquations.clear(); -} - -template -void DOFBlockerLMConstraint::buildConstraintMatrix(const core::ConstraintParams* /*cParams*/, core::MultiMatrixDerivId cId, unsigned int &cIndex) -{ - if(!idxEquations.empty() ) return; - - using namespace core::objectmodel; - Data* dC = cId[this->constrainedObject1].write(); - helper::WriteAccessor > c = *dC; - - const SetIndexArray &indices = f_indices.getValue(); - const type::vector &axis=BlockedAxis.getValue(); - idxEquations.resize(indices.size()); - unsigned int numParticle=0; - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it, ++numParticle) - { - const Index index=*it; - for (unsigned int i=0; iwriteLine(cIndex).addCol(index,axis[i]); - idxEquations[numParticle].push_back(cIndex++); - } - } - - -} - -template -void DOFBlockerLMConstraint::writeConstraintEquations(unsigned int& lineNumber, core::MultiVecId id, ConstOrder Order) -{ - using namespace core; - using namespace core::objectmodel; - //We don't constrain the Position, only the velocities and accelerations - if (idxEquations.empty() || - Order==core::ConstraintParams::POS) return; - - const SetIndexArray & indices = f_indices.getValue(); - const type::vector &factor=factorAxis.getValue(); - - for (unsigned int numParticle=0; numParticleaddGroupConstraint(Order); - SReal correction=0; - switch(Order) - { - case core::ConstraintParams::ACC : - case core::ConstraintParams::VEL : - { - ConstVecId v1 = id.getId(this->constrainedObject1); - correction = this->constrainedObject1->getConstraintJacobianTimesVecDeriv(idxEquations[numParticle][i],v1); - break; - } - default: break; - }; - if (!factor.empty()) - { - if (i < factor.size()) correction*=factor[i]; - else correction*=factor.back(); - } - constraint->addConstraint( lineNumber, idxEquations[numParticle][i], -correction); - } - } -} - -template -void DOFBlockerLMConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowForceFields()) return; - const VecCoord& x =this->constrainedObject1->read(core::ConstVecCoordId::position())->getValue(); - - vparams->drawTool()->saveLastState(); - - const SetIndexArray & indices = f_indices.getValue(); - sofa::type::RGBAColor color = sofa::type::RGBAColor::yellow(); - - for (SetIndexArray::const_iterator it = indices.begin(); - it != indices.end(); - ++it) - { - Index index=(*it); - Coord pos=x[index]; - type::Vector3 position; - DataTypes::get(position[0], position[1], position[2], pos); - const type::vector& axis=BlockedAxis.getValue(); - for (unsigned int i=0; idrawTool()->drawArrow(position, position+direction*showSizeAxis.getValue(), - showSizeAxis.getValue()*0.03,color); - } - } - vparams->drawTool()->restoreLastState(); -} - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.cpp b/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.cpp deleted file mode 100644 index 2c007661bfc..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/****************************************************************************** -* 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_DISTANCELMCONSTRAINT_CPP -#include - -#include -#include -#include -#include - -namespace sofa::component::constraintset -{ - -using namespace sofa::type; -using namespace sofa::defaulttype; -using namespace sofa::helper; - -///TODO: handle combinaison of Rigid and Deformable bodies. - -int DistanceLMConstraintClass = core::RegisterObject("Maintain constant the length of some edges of a pair of objects") - .add< DistanceLMConstraint >() - .add< DistanceLMConstraint >() - - ; - -template class LMCONSTRAINT_API DistanceLMConstraint; -template class LMCONSTRAINT_API DistanceLMConstraint; - - - -//TODO(dmarchal) Yet again this ugly code duplication between float and double. -// To fix this you can use the same design of UniformMass. -template<> -Rigid3Types::Deriv DistanceLMConstraint::getDirection(const Edge &e, const VecCoord &x1, const VecCoord &x2) const -{ - Vector3 V12=(x2[e[1]].getCenter() - x1[e[0]].getCenter()); V12.normalize(); - return Deriv(V12, Vector3()); -} -template<> -void DistanceLMConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (this->l0.size() != vecConstraint.getValue().size()) updateRestLength(); - - if (vparams->displayFlags().getShowBehaviorModels()) - { - const VecCoord &x1= this->constrainedObject1->read(core::ConstVecCoordId::position())->getValue(); - const VecCoord &x2= this->constrainedObject2->read(core::ConstVecCoordId::position())->getValue(); - - std::vector< Vector3 > points; - const SeqEdges &edges = vecConstraint.getValue(); - for (unsigned int i=0; idrawTool()->drawLines(points, 1, sofa::type::RGBAColor::green()); - } -} - - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.h b/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.h deleted file mode 100644 index ce300c28204..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.h +++ /dev/null @@ -1,114 +0,0 @@ -/****************************************************************************** -* 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::constraintset -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class DistanceLMConstraintInternalData -{ -}; - - - - -/** Keep two particules at an initial distance -*/ -template -class DistanceLMConstraint : public core::behavior::LMConstraint -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(DistanceLMConstraint,DataTypes),SOFA_TEMPLATE2(sofa::core::behavior::LMConstraint, DataTypes, DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename core::behavior::MechanicalState MechanicalState; - - typedef typename sofa::core::topology::BaseMeshTopology::SeqEdges SeqEdges; - typedef typename sofa::core::topology::BaseMeshTopology::Edge Edge; - - typedef core::ConstraintParams::ConstOrder ConstOrder; - -protected: - DistanceLMConstraintInternalData data; - friend class DistanceLMConstraintInternalData; - - DistanceLMConstraint(); - DistanceLMConstraint( MechanicalState *dof); - DistanceLMConstraint( MechanicalState *dof1, MechanicalState * dof2); - - ~DistanceLMConstraint() {} -public: - void init() override; - void reinit() override; - - // -- LMConstraint interface - - void buildConstraintMatrix(const core::ConstraintParams* cParams, core::MultiMatrixDerivId cId, unsigned int &cIndex) override; - void writeConstraintEquations(unsigned int& lineNumber, core::MultiVecId id, ConstOrder order) override; - - void draw(const core::visual::VisualParams* vparams) override; - - bool isCorrectionComputedWithSimulatedDOF(core::ConstraintParams::ConstOrder /*order*/) const override; - - //Edges involving a distance constraint - Data< SeqEdges > vecConstraint; ///< List of the edges to constrain - - /// Link to be set to the topology container in the component graph. - SingleLink , sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; -protected : - - ///Compute the length of an edge given the vector of coordinates corresponding - double lengthEdge(const Edge &e, const VecCoord &x1,const VecCoord &x2) const; - ///Compute the direction of the constraint - Deriv getDirection(const Edge &e, const VecCoord &x1, const VecCoord &x2) const; - void updateRestLength(); - - type::vector< unsigned int > registeredConstraints; - - // rest length pre-computated - sofa::type::vector< double > l0; -}; - -template<> -defaulttype::Rigid3Types::Deriv DistanceLMConstraint::getDirection(const Edge &e, const VecCoord &x1, const VecCoord &x2) const; - - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_DISTANCELMCONSTRAINT_CPP) -extern template class LMCONSTRAINT_API DistanceLMConstraint; -extern template class LMCONSTRAINT_API DistanceLMConstraint; - -#endif - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.inl b/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.inl deleted file mode 100644 index 0e559f5e3e4..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMConstraint.inl +++ /dev/null @@ -1,234 +0,0 @@ -/****************************************************************************** -* 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::constraintset -{ - -template -DistanceLMConstraint::DistanceLMConstraint( MechanicalState *dof1, MechanicalState * dof2) - : core::behavior::LMConstraint(dof1,dof2) - , vecConstraint(sofa::core::objectmodel::Base::initData(&vecConstraint, "vecConstraint", "List of the edges to constrain")) - , l_topology(initLink("topology", "link to the topology container")) -{ -} - -template -DistanceLMConstraint::DistanceLMConstraint( MechanicalState *dof) - : DistanceLMConstraint(dof,dof) -{ -} - -template -DistanceLMConstraint::DistanceLMConstraint() - : DistanceLMConstraint(nullptr, nullptr) -{ -} - -template -void DistanceLMConstraint::init() -{ - sofa::core::behavior::LMConstraint::init(); - - // TODO epenod 2019-12-05: Adapt code to not look for topology if constraint is manually set. Need to dig more in the code to understand how topology is used. - if (vecConstraint.getValue().size() != 0) - { - // nothing to do in this case - return; - } - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - core::topology::BaseMeshTopology *_topology = l_topology.get(); - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - if (!_topology) - { - msg_error() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - - if (vecConstraint.getValue().size() == 0 && (this->constrainedObject1==this->constrainedObject2) ) - vecConstraint.setValue(_topology->getEdges()); -} - -template -void DistanceLMConstraint::reinit() -{ - updateRestLength(); -} - -template -double DistanceLMConstraint::lengthEdge(const Edge &e, const VecCoord &x1, const VecCoord &x2) const -{ - return (x2[e[1]] - x1[e[0]]).norm(); -} - -template -void DistanceLMConstraint::updateRestLength() -{ - const VecCoord &x0_1= this->constrainedObject1->read(core::ConstVecCoordId::position())->getValue(); - const VecCoord &x0_2= this->constrainedObject2->read(core::ConstVecCoordId::position())->getValue(); - const SeqEdges &edges = vecConstraint.getValue(); - this->l0.resize(edges.size()); - for (unsigned int i=0; il0[i] = lengthEdge(edges[i],x0_1,x0_2); - } -} - -template -typename DataTypes::Deriv DistanceLMConstraint::getDirection(const Edge &e, const VecCoord &x1, const VecCoord &x2) const -{ - Deriv V12 = (x2[e[1]] - x1[e[0]]); - V12.normalize(); - return V12; -} - - -template -void DistanceLMConstraint::buildConstraintMatrix(const core::ConstraintParams* cParams, core::MultiMatrixDerivId cId, unsigned int &cIndex) -{ - Data* dC1 = cId[this->constrainedObject1].write(); - helper::WriteAccessor > c1 = *dC1; - - Data* dC2 = cId[this->constrainedObject2].write(); - helper::WriteAccessor > c2 = *dC2; - - helper::ReadAccessor > x1 = *cParams->readX(this->constrainedObject1); - helper::ReadAccessor > x2 = *cParams->readX(this->constrainedObject2); - - const SeqEdges &edges = vecConstraint.getValue(); - - if (this->l0.size() != edges.size()) updateRestLength(); - - registeredConstraints.clear(); - - for (unsigned int i = 0; i < edges.size(); ++i) - { - unsigned int idx1 = edges[i][0]; - unsigned int idx2 = edges[i][1]; - - const Deriv V12 = getDirection(edges[i], x1.ref() , x2.ref() ); - - MatrixDerivRowIterator c1_it = c1->writeLine(cIndex); - c1_it.addCol(idx1, V12); - MatrixDerivRowIterator c2_it = c2->writeLine(cIndex); - c2_it.addCol(idx2, -V12); - - registeredConstraints.push_back(cIndex); - cIndex++; - } -} - -template -void DistanceLMConstraint::writeConstraintEquations(unsigned int& lineNumber, core::MultiVecId id, ConstOrder Order) -{ - using namespace core; - using namespace core::objectmodel; - const SeqEdges &edges = vecConstraint.getValue(); - - if (registeredConstraints.empty()) return; - for (unsigned int i=0; iaddGroupConstraint(Order); - SReal correction=0; - switch(Order) - { - case core::ConstraintParams::ACC : - case core::ConstraintParams::VEL : - { - ConstVecId v1 = id.getId(this->constrainedObject1); - ConstVecId v2 = id.getId(this->constrainedObject2); - correction = this->constrainedObject1->getConstraintJacobianTimesVecDeriv(registeredConstraints[i],v1); - correction += this->constrainedObject2->getConstraintJacobianTimesVecDeriv(registeredConstraints[i],v2); - break; - } - case core::ConstraintParams::POS : - case core::ConstraintParams::POS_AND_VEL : - { - const VecCoord &x1 = this->constrainedObject1->read(core::ConstVecCoordId(id.getId(this->constrainedObject1) ))->getValue(); - const VecCoord &x2 = this->constrainedObject2->read(core::ConstVecCoordId(id.getId(this->constrainedObject2) ))->getValue(); - SReal length = lengthEdge(edges[i],x1,x2); - SReal restLength = this->l0[i]; - correction= restLength-length; - break; - } - }; - constraint->addConstraint( lineNumber, registeredConstraints[i], -correction); - } -} - - -template <> -void DistanceLMConstraint::draw(const core::visual::VisualParams* vparams); - - -template -void DistanceLMConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (this->l0.size() != vecConstraint.getValue().size()) updateRestLength(); - - vparams->drawTool()->saveLastState(); - if (vparams->displayFlags().getShowBehaviorModels()) - { - const VecCoord &x1= this->constrainedObject1->read(core::ConstVecCoordId::position())->getValue(); - const VecCoord &x2= this->constrainedObject2->read(core::ConstVecCoordId::position())->getValue(); - - std::vector< sofa::type::Vector3 > points; - const SeqEdges &edges = vecConstraint.getValue(); - for (unsigned int i=0; idrawTool()->drawLines(points, 1, sofa::type::RGBAColor::green()); - } - vparams->drawTool()->restoreLastState(); -} - -template -bool DistanceLMConstraint::isCorrectionComputedWithSimulatedDOF(core::ConstraintParams::ConstOrder /*order*/) const -{ - simulation::Node* node1=(simulation::Node*) this->constrainedObject1->getContext(); - simulation::Node* node2=(simulation::Node*) this->constrainedObject2->getContext(); - if (node1->mechanicalMapping.empty() && node2->mechanicalMapping.empty()) return true; - else return false; -} - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.cpp b/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.cpp deleted file mode 100644 index da6b2994fa1..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/****************************************************************************** -* 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_DistanceLMContactConstraint_CPP -#include - -#include -#include -#include - -namespace sofa::component::constraintset -{ - -using namespace sofa::defaulttype; -using namespace sofa::helper; - -int DistanceLMContactConstraintClass = core::RegisterObject("Maintain a minimum contact distance between two objects") - .add< DistanceLMContactConstraint >() - - ; - -template class LMCONSTRAINT_API DistanceLMContactConstraint; - - - - - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.h b/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.h deleted file mode 100644 index 97c5c0abe2f..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.h +++ /dev/null @@ -1,147 +0,0 @@ -/****************************************************************************** -* 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::constraintset -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class DistanceLMContactConstraintInternalData -{ -}; - - - - -/** Keep two particules at an initial distance -*/ -template -class DistanceLMContactConstraint : public core::behavior::LMConstraint, public ContactDescriptionHandler -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(DistanceLMContactConstraint,DataTypes),SOFA_TEMPLATE2(sofa::core::behavior::LMConstraint, DataTypes, DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - - typedef typename core::behavior::MechanicalState MechanicalState; - typedef typename sofa::core::topology::BaseMeshTopology::SeqEdges SeqEdges; - typedef typename sofa::core::topology::BaseMeshTopology::Edge Edge; - typedef core::ConstraintParams::ConstOrder ConstOrder; - typedef core::behavior::ConstraintGroup ConstraintGroup; - - -protected: - DistanceLMContactConstraint(); - DistanceLMContactConstraint( MechanicalState *dof); - DistanceLMContactConstraint( MechanicalState *dof1, MechanicalState * dof2); - - - ~DistanceLMContactConstraint() override {} -public: - // -- LMConstraint interface - void buildConstraintMatrix(const core::ConstraintParams* cParams, core::MultiMatrixDerivId cId, unsigned int &cIndex) override; - - void writeConstraintEquations(unsigned int& lineNumber, core::MultiVecId id, ConstOrder order) override; - void LagrangeMultiplierEvaluation(const SReal* Wptr, const SReal* cptr, SReal* LambdaInitptr, - core::behavior::ConstraintGroup * group) override; - - bool isCorrectionComputedWithSimulatedDOF(ConstOrder order) const override; - void clear(); - /// register a new contact - void addContact(unsigned m1, unsigned m2); - void draw(const core::visual::VisualParams* vparams) override; - -protected : - - /// Each scalar constraint (up to three per contact) has an associated index - type::vector< unsigned int > scalarConstraintsIndices; - -public: - /// Contacts are represented by pairs of point indices - Data< SeqEdges > pointPairs; - - /// Friction coefficients (same for all contacts) - Data< SReal > contactFriction; - -protected: - ///Compute the length of an edge given the vector of coordinates corresponding - double lengthEdge(const Edge &e, const VecCoord &x1,const VecCoord &x2) const; - /// Contact normal - Deriv computeNormal(const Edge &e, const VecCoord &x1, const VecCoord &x2) const; - /// Contact tangent vectors - void computeTangentVectors( Deriv& T1, Deriv& T2, const Deriv& N ); - - struct Contact - { - //Constrained Axis - Deriv n,t1,t2; - Contact() {} - Contact( Deriv norm, Deriv tgt1, Deriv tgt2 ):n(norm),t1(tgt1),t2(tgt2),contactForce(Deriv()) {} - Deriv contactForce; - }; - - std::map< Edge, Contact > edgeToContact; - std::map< ConstraintGroup*, Contact* > constraintGroupToContact; - core::collision::Intersection* intersection; -protected: - DistanceLMContactConstraintInternalData data; - friend class DistanceLMContactConstraintInternalData; - - - void initColorContactState() - { - colorsContactState.clear(); - //Vanishing - colorsContactState.push_back(sofa::type::RGBAColor(0.0f,1.0f,0.0f,1.0)); - //Sticking - colorsContactState.push_back(sofa::type::RGBAColor(1.0f,0.0f,0.0f,1.0)); - //Sliding - colorsContactState.push_back(sofa::type::RGBAColor(1.0f,1.0f,0.0f,1.0)); - //Sliding Direction - colorsContactState.push_back(sofa::type::RGBAColor(1.0f,0.0f,1.0f,1.0)); - } - - type::vector< sofa::type::RGBAColor > colorsContactState; -}; - - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_DistanceLMContactConstraint_CPP) -extern template class LMCONSTRAINT_API DistanceLMContactConstraint; - -#endif - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.inl b/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.inl deleted file mode 100644 index 9f2639c162b..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/DistanceLMContactConstraint.inl +++ /dev/null @@ -1,380 +0,0 @@ -/****************************************************************************** -* 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 -#include - - -namespace sofa::component::constraintset -{ - - -template -DistanceLMContactConstraint::DistanceLMContactConstraint() - : DistanceLMContactConstraint(nullptr, nullptr) -{ -} - -template -DistanceLMContactConstraint::DistanceLMContactConstraint( MechanicalState *dof) - : DistanceLMContactConstraint(dof, dof) -{ -} - -template -DistanceLMContactConstraint::DistanceLMContactConstraint( MechanicalState *dof1, MechanicalState * dof2) - : core::behavior::LMConstraint(dof1,dof2) - , pointPairs(sofa::core::objectmodel::Base::initData(&pointPairs, "pointPairs", "List of the edges to constrain")) - , contactFriction(sofa::core::objectmodel::Base::initData(&contactFriction, "contactFriction", "Coulomb friction coefficient (same for all)")) - , intersection(0) -{ - initColorContactState(); -} - - -template -double DistanceLMContactConstraint::lengthEdge(const Edge &e, const VecCoord &x1, const VecCoord &x2) const -{ - return (x2[e[1]] - x1[e[0]]).norm(); -} - -template -void DistanceLMContactConstraint::clear() -{ - pointPairs.beginEdit()->clear(); - pointPairs.endEdit(); -} - -template -void DistanceLMContactConstraint::addContact(unsigned m1, unsigned m2) -{ - pointPairs.beginEdit()->push_back(Edge(m1,m2)); - pointPairs.endEdit(); -} - - -template -typename DataTypes::Deriv DistanceLMContactConstraint::computeNormal(const Edge &e, const VecCoord &x1, const VecCoord &x2) const -{ - //Deriv V12 = (x2[e[1]] - x1[e[0]]); - Deriv V12 = (x1[e[0]] - x2[e[1]]); - V12.normalize(); - return V12; -} - -// Compute T1 and T2, normalized and orthogonal to N. N must be normalized already. -template -void DistanceLMContactConstraint::computeTangentVectors( Deriv& T1, Deriv& T2, const Deriv& N ) -{ - T1 = cross( N,Deriv(1,0,0) ); - if (dot(T1,T1) < 1.0e-2) T1=cross( N,Deriv(0,1,0) ); - T1.normalize(); - T2 = N.cross(T1); -} - - -template -void DistanceLMContactConstraint::buildConstraintMatrix(const core::ConstraintParams* cParams, core::MultiMatrixDerivId cId, unsigned int &cIndex) -{ - using namespace core::objectmodel; - Data* dC1 = cId[this->constrainedObject1].write(); - helper::WriteAccessor > c1 = *dC1; - - Data* dC2 = cId[this->constrainedObject2].write(); - helper::WriteAccessor > c2 = *dC2; - - helper::ReadAccessor > x1 = *cParams->readX(this->constrainedObject1); - helper::ReadAccessor > x2 = *cParams->readX(this->constrainedObject2); - - const SeqEdges &edges = pointPairs.getValue(); - - scalarConstraintsIndices.clear(); - constraintGroupToContact.clear(); - edgeToContact.clear(); - - for (unsigned int i=0; iwriteLine(cIndex); - c1_normal.addCol(idx1,normal); - MatrixDerivRowIterator c2_normal = c2->writeLine(cIndex); - c2_normal.addCol(idx2,-normal); - scalarConstraintsIndices.push_back(cIndex++); - - Deriv tgt1, tgt2; - computeTangentVectors(tgt1,tgt2,normal); - - MatrixDerivRowIterator c1_t1 = c1->writeLine(cIndex); - c1_t1.addCol(idx1,tgt1); - MatrixDerivRowIterator c2_t1 = c2->writeLine(cIndex); - c2_t1.addCol(idx2,-tgt1); - scalarConstraintsIndices.push_back(cIndex++); - - - MatrixDerivRowIterator c1_t2 = c1->writeLine(cIndex); - c1_t2.addCol(idx1,tgt2); - MatrixDerivRowIterator c2_t2 = c2->writeLine(cIndex); - c2_t2.addCol(idx2,-tgt2); - scalarConstraintsIndices.push_back(cIndex++); - - edgeToContact[edges[i]] = Contact(normal,tgt1,tgt2); - } - -} -template -void DistanceLMContactConstraint::writeConstraintEquations(unsigned int& lineNumber, core::MultiVecId id, ConstOrder Order) -{ - using namespace core; - using namespace core::objectmodel; - const SeqEdges &edges = pointPairs.getValue(); - - if (scalarConstraintsIndices.empty()) return; - unsigned scalarConstraintIndex = 0; - for (unsigned int i=0; isimulatedObject1); - ConstVecId v2 = id.getId(this->simulatedObject2); - core::behavior::ConstraintGroup *constraintGroup = this->addGroupConstraint(Order); - constraintGroupToContact[constraintGroup] = &edgeToContact[edges[i]]; - - SReal correction = 0; - correction+= this->simulatedObject1->getConstraintJacobianTimesVecDeriv(scalarConstraintsIndices[scalarConstraintIndex],v1); - correction+= this->simulatedObject2->getConstraintJacobianTimesVecDeriv(scalarConstraintsIndices[scalarConstraintIndex],v2); - constraintGroup->addConstraint( lineNumber, scalarConstraintsIndices[scalarConstraintIndex++], -correction); - - correction = 0; - correction+= this->simulatedObject1->getConstraintJacobianTimesVecDeriv(scalarConstraintsIndices[scalarConstraintIndex],v1); - correction+= this->simulatedObject2->getConstraintJacobianTimesVecDeriv(scalarConstraintsIndices[scalarConstraintIndex],v2); - constraintGroup->addConstraint( lineNumber, scalarConstraintsIndices[scalarConstraintIndex++], -correction); - - correction = 0; - correction+= this->simulatedObject1->getConstraintJacobianTimesVecDeriv(scalarConstraintsIndices[scalarConstraintIndex],v1); - correction+= this->simulatedObject2->getConstraintJacobianTimesVecDeriv(scalarConstraintsIndices[scalarConstraintIndex],v2); - constraintGroup->addConstraint( lineNumber, scalarConstraintsIndices[scalarConstraintIndex++], -correction); - break; - } - case core::ConstraintParams::POS : - case core::ConstraintParams::POS_AND_VEL : - { - SReal minDistance = 0; - - if (!intersection) - this->getContext()->get(intersection); - - if (intersection) - minDistance = intersection->getContactDistance(); - else - msg_error() << "No intersection component found!!"; - - const VecCoord &x1 = this->constrainedObject1->read(core::ConstVecCoordId(id.getId(this->constrainedObject1)))->getValue(); - const VecCoord &x2 = this->constrainedObject2->read(core::ConstVecCoordId(id.getId(this->constrainedObject2)))->getValue(); - - SReal correction = minDistance-lengthEdge(edges[i],x1,x2); //Distance min-current length - - if (correction>0) - { - core::behavior::ConstraintGroup *constraintGroup = this->addGroupConstraint(Order); - constraintGroup->addConstraint( lineNumber, scalarConstraintsIndices[scalarConstraintIndex], correction); - } - - scalarConstraintIndex += 3; - break; - } - }; - - } -} - -template -void DistanceLMContactConstraint::LagrangeMultiplierEvaluation(const SReal* W, const SReal* c, SReal* Lambda, - core::behavior::ConstraintGroup * group) -{ - switch (group->getOrder()) - { - case core::ConstraintParams::ACC : - case core::ConstraintParams::VEL : - { - Contact &out=*(this->constraintGroupToContact[group]); - ContactDescription &contact=this->getContactDescription(group); - - //The force cannot be attractive! - if (Lambda[0] <= 0) - { - contact.state=VANISHING; - group->setActive(false); - out.contactForce=Deriv(); - return; - } - - //Friction force - - const SReal normTangentForce=sqrt(pow(Lambda[1],2)+pow(Lambda[2],2)); - const SReal normNormalForce=fabs(Lambda[0]); - - const SReal& coeffFriction = contactFriction.getValue(); - - //Test if we are outside the coulomb friction cone - if ( (normTangentForce / normNormalForce) > coeffFriction) - { - contact.state=SLIDING; - //Force applied - out.contactForce = out.n*Lambda[0]+out.t1*Lambda[1]+ out.t2*Lambda[2]; - //Outside: we project the force to the cone - - //directionCone <--> n' : unitary vector along the cone - const SReal factor=coeffFriction*normNormalForce/normTangentForce; - - Deriv directionCone=out.n*Lambda[0]+(out.t1*Lambda[1]+ out.t2*Lambda[2])*factor; - directionCone.normalize(); - - contact.coeff[0]=out.n *directionCone; - contact.coeff[1]=out.t1 *directionCone; - contact.coeff[2]=out.t2 *directionCone; - - const SReal value=W[0]*contact.coeff[0]+ - W[1]*contact.coeff[1] + - W[2]*contact.coeff[2]; - - if (value == 0) - { - msg_error() << "DIVISION BY ZERO AVOIDED: w=[" << W[0] << "," << W[1] << "," << W[2] << "] " << " DIRECTION CONE: " << directionCone << " BARY COEFF: " << contact.coeff[0] << ", " << contact.coeff[1] << ", " << contact.coeff[2]; - group->setActive(false); - out.contactForce=Deriv(); - return; - } - const SReal slidingLambda=c[0]/value; - out.contactForce = directionCone*slidingLambda; - //Then project the force to the border of the cone - Lambda[0]=out.contactForce*out.n; - Lambda[1]=out.contactForce*out.t1; - Lambda[2]=out.contactForce*out.t2; - } - else contact.state=STICKING; - - out.contactForce = out.n*Lambda[0]+out.t1*Lambda[1]+out.t2*Lambda[2]; - - - break; - } - case core::ConstraintParams::POS : - case core::ConstraintParams::POS_AND_VEL : - { - //The force cannot be attractive! - if (Lambda[0] < 0) - { - group->setActive(false); - return; - } - } - default: {} - } - - return; -} - - -template -bool DistanceLMContactConstraint::isCorrectionComputedWithSimulatedDOF(ConstOrder order) const -{ - switch(order) - { - case core::ConstraintParams::ACC : - case core::ConstraintParams::VEL : - return true; - case core::ConstraintParams::POS : - case core::ConstraintParams::POS_AND_VEL : - return false; - } - return false; -} - -template -void DistanceLMContactConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (vparams->displayFlags().getShowBehaviorModels()) - { - vparams->drawTool()->saveLastState(); - - const VecCoord &x1= this->constrainedObject1->read(core::ConstVecCoordId::position())->getValue(); - const VecCoord &x2= this->constrainedObject2->read(core::ConstVecCoordId::position())->getValue(); - - - sofa::type::vector< sofa::type::vector< sofa::type::Vector3 > > points; - points.resize(3); - //Sliding: show direction of the new constraint - sofa::type::vector< sofa::type::Vector3 > slidingConstraints; - - const sofa::type::vector< ConstraintGroup* > &groups = this->getConstraintsOrder(core::ConstraintParams::VEL); - - const SeqEdges &edges = pointPairs.getValue(); - for (unsigned int i=0; igetContactDescription(groups[i]); - Contact &contactDirection=*(this->constraintGroupToContact[groups[i]]); - - points[contactDescription.state].push_back(x1[edges[i][0]]); - points[contactDescription.state].push_back(x2[edges[i][1]]); - - if (contactDescription.state == SLIDING) - { - sofa::type::Vector3 direction=contactDirection.n *contactDescription.coeff[0] + - contactDirection.t1*contactDescription.coeff[1]+ - contactDirection.t2*contactDescription.coeff[2]; - - direction.normalize(); - SReal sizeV=this->lengthEdge(edges[i], x1,x2); - - slidingConstraints.push_back(x1[edges[i][0]]); - slidingConstraints.push_back(x1[edges[i][0]]-direction*sizeV); - } - } - - vparams->drawTool()->drawLines(points[VANISHING], 1, colorsContactState[VANISHING]); - vparams->drawTool()->drawLines(points[STICKING], 1, colorsContactState[STICKING]); - vparams->drawTool()->drawLines(points[SLIDING], 1, colorsContactState[SLIDING]); - - vparams->drawTool()->drawLines(slidingConstraints, 1, colorsContactState.back()); - - vparams->drawTool()->restoreLastState(); - - } -} - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.cpp b/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.cpp deleted file mode 100644 index 217fc79505a..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/****************************************************************************** -* 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_FIXEDLMCONSTRAINT_CPP -#include -#include -#include -#include - - -namespace sofa::component::constraintset -{ - -using namespace sofa::defaulttype; -using namespace sofa::helper; - -int FixedLMConstraintClass = core::RegisterObject("Maintain a set of particle to a fixed position using LMConstraint") - .add< FixedLMConstraint >() - .add< FixedLMConstraint >() - - ; - -template class LMCONSTRAINT_API FixedLMConstraint; -template class LMCONSTRAINT_API FixedLMConstraint; - - - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.h b/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.h deleted file mode 100644 index f87facefa0a..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.h +++ /dev/null @@ -1,117 +0,0 @@ -/****************************************************************************** -* 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::constraintset -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class FixedLMConstraintInternalData -{ -}; - - - -/** Keep two particules at an initial distance -*/ -template -class FixedLMConstraint : public core::behavior::LMConstraint -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(FixedLMConstraint,DataTypes),SOFA_TEMPLATE2(sofa::core::behavior::LMConstraint, DataTypes, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename core::behavior::MechanicalState MechanicalState; - - - typedef type::vector SetIndexArray; - typedef sofa::component::topology::TopologySubsetIndices SetIndex; - - typedef core::ConstraintParams::ConstOrder ConstOrder; - -protected: - FixedLMConstraintInternalData data; - friend class FixedLMConstraintInternalData; - - FixedLMConstraint( MechanicalState *dof = nullptr); - - ~FixedLMConstraint() - { - - } - -public: - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - void init() override; - void draw(const core::visual::VisualParams* vparams) override; - void initFixedPosition(); - void reset() override {initFixedPosition();} - - // -- LMConstraint interface - void buildConstraintMatrix(const core::ConstraintParams* cParams, core::MultiMatrixDerivId cId, unsigned int &cIndex) override; - void writeConstraintEquations(unsigned int& lineNumber, core::MultiVecId id, ConstOrder order) override; - - bool isCorrectionComputedWithSimulatedDOF(ConstOrder /*order*/) const override - { - simulation::Node* node=(simulation::Node*) this->constrainedObject1->getContext(); - if (node->mechanicalMapping.empty()) return true; - else return false; - } - - SetIndex f_indices; ///< List of the index of particles to be fixed - Data _drawSize; ///< 0 -> point based rendering, >0 -> radius of spheres - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected : - - Deriv X,Y,Z; - SetIndexArray idxX, idxY, idxZ; - std::map< Index, Coord> restPosition; -}; - - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_FIXEDLMCONSTRAINT_CPP) -extern template class LMCONSTRAINT_API FixedLMConstraint; -extern template class LMCONSTRAINT_API FixedLMConstraint; - -#endif - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.inl b/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.inl deleted file mode 100644 index e1a6f364c66..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/FixedLMConstraint.inl +++ /dev/null @@ -1,219 +0,0 @@ -/****************************************************************************** -* 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 - - -namespace sofa::component::constraintset -{ - -template -FixedLMConstraint::FixedLMConstraint(MechanicalState *dof) - : core::behavior::LMConstraint(dof,dof) - , f_indices(core::objectmodel::Base::initData(&f_indices, "indices", "List of the index of particles to be fixed")) - , _drawSize(core::objectmodel::Base::initData(&_drawSize,0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) - , l_topology(initLink("topology", "link to the topology container")) -{ - -} - -template -void FixedLMConstraint::clearConstraints() -{ - f_indices.beginEdit()->clear(); - f_indices.endEdit(); -} - -template -void FixedLMConstraint::addConstraint(Index index) -{ - f_indices.beginEdit()->push_back(index); - f_indices.endEdit(); -} - -template -void FixedLMConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*f_indices.beginEdit(),index); - f_indices.endEdit(); -} - - -template -void FixedLMConstraint::initFixedPosition() -{ - this->restPosition.clear(); - const VecCoord& x =this->constrainedObject1->read(core::ConstVecCoordId::position())->getValue(); - const SetIndexArray & indices = this->f_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - Index index=*it; - this->restPosition.insert(std::make_pair(index, x[index])); - } -} - -template -void FixedLMConstraint::init() -{ - core::behavior::LMConstraint::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - sofa::core::topology::BaseMeshTopology* _topology = l_topology.get(); - - if (_topology) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological change handling - f_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - - X[0]=1; X[1]=0; X[2]=0; - Y[0]=0; Y[1]=1; Y[2]=0; - Z[0]=0; Z[1]=0; Z[2]=1; - - initFixedPosition(); - -} - - - -template -void FixedLMConstraint::buildConstraintMatrix(const core::ConstraintParams* /* cParams*/, core::MultiMatrixDerivId cId, unsigned int &cIndex) -{ - using namespace core::objectmodel; - Data* dC = cId[this->constrainedObject1].write(); - helper::WriteAccessor > c = *dC; - idxX.clear(); - idxY.clear(); - idxZ.clear(); - const SetIndexArray &indices = f_indices.getValue(); - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - const Index index=*it; - - //Constraint degree of freedom along X direction - c->writeLine(cIndex).addCol(index,X); - idxX.push_back(cIndex++); - - //Constraint degree of freedom along X direction - c->writeLine(cIndex).addCol(index,Y); - idxY.push_back(cIndex++); - - //Constraint degree of freedom along Z direction - c->writeLine(cIndex).addCol(index,Z); - idxZ.push_back(cIndex++); - } -} - -template -void FixedLMConstraint::writeConstraintEquations(unsigned int& lineNumber, core::MultiVecId id, ConstOrder Order) -{ - using namespace core; - using namespace core::objectmodel; - const SetIndexArray & indices = f_indices.getValue(); - - unsigned int counter=0; - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it,++counter) - { - const Index index = *it; - - core::behavior::ConstraintGroup *constraint = this->addGroupConstraint(Order); - SReal correctionX=0,correctionY=0,correctionZ=0; - switch(Order) - { - case core::ConstraintParams::ACC : - case core::ConstraintParams::VEL : - { - ConstVecId v1 = id.getId(this->constrainedObject1); - correctionX = this->constrainedObject1->getConstraintJacobianTimesVecDeriv(idxX[counter],v1); - correctionY = this->constrainedObject1->getConstraintJacobianTimesVecDeriv(idxY[counter],v1); - correctionZ = this->constrainedObject1->getConstraintJacobianTimesVecDeriv(idxZ[counter],v1); - break; - } - case core::ConstraintParams::POS : - case core::ConstraintParams::POS_AND_VEL : - { - ConstVecId xid = id.getId(this->constrainedObject1); - helper::ReadAccessor > x = *this->constrainedObject1->read((ConstVecCoordId)xid); - - //If a new particle has to be fixed, we add its current position as rest position - if (restPosition.find(index) == this->restPosition.end()) - { - restPosition.insert(std::make_pair(index, x[index])); - } - - - Coord v=x[index]-restPosition[index]; - correctionX=v[0]; - correctionY=v[1]; - correctionZ=v[2]; - break; - } - }; - - constraint->addConstraint( lineNumber, idxX[counter], -correctionX); - constraint->addConstraint( lineNumber, idxY[counter], -correctionY); - constraint->addConstraint( lineNumber, idxZ[counter], -correctionZ); - } -} - - - -template -void FixedLMConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x =this->constrainedObject1->read(core::ConstVecCoordId::position())->getValue(); - const SetIndexArray & indices = f_indices.getValue(); - - std::vector< type::Vector3 > points; - type::Vector3 point; - for (unsigned int index : indices) - { - point = DataTypes::getCPos(x[index]); - points.push_back(point); - } - if( _drawSize.getValue() == 0) // old classical drawing by points - { - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); - } - else - { - vparams->drawTool()->drawSpheres(points, (float)_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); - } -} - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.cpp b/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.cpp deleted file mode 100644 index cddf0e38c73..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/****************************************************************************** -* 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_CORE_BEHAVIOR_LMCONSTRAINT_CPP -#include -#include -#include - -namespace sofa -{ - -namespace core -{ - -namespace behavior -{ - -using namespace sofa::defaulttype; - -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; - -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; - -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; - -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; - -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; -template class LMCONSTRAINT_API LMConstraint; - - -//Need the combinations - -} // namespace behavior - -} // namespace core - -} // namespace sofa diff --git a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.h b/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.h deleted file mode 100644 index 522ea3c859d..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.h +++ /dev/null @@ -1,189 +0,0 @@ -/****************************************************************************** -* 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 * -******************************************************************************/ -#ifndef SOFA_CORE_BEHAVIOR_LMCONSTRAINT_H -#define SOFA_CORE_BEHAVIOR_LMCONSTRAINT_H - -#include -#include -#include - -namespace sofa -{ - -namespace core -{ - -namespace behavior -{ - -/** - * \brief Component computing constraints within a simulated body. - * - * This class define the abstract API common to constraints using a given type - * of DOFs. - * A LMConstraint computes complex constraints applied to two bodies - * - */ -template -class LMConstraint : public BaseLMConstraint -{ -public: - SOFA_ABSTRACT_CLASS(SOFA_TEMPLATE2(LMConstraint,DataTypes1,DataTypes2), BaseLMConstraint); - - typedef typename DataTypes1::Real Real1; - typedef typename DataTypes1::VecCoord VecCoord1; - typedef typename DataTypes1::VecDeriv VecDeriv1; - typedef typename DataTypes1::Coord Coord1; - typedef typename DataTypes1::Deriv Deriv1; - typedef typename DataTypes1::MatrixDeriv MatrixDeriv1; - typedef typename DataTypes1::MatrixDeriv::RowConstIterator MatrixDeriv1RowConstIterator; - typedef typename DataTypes1::MatrixDeriv::ColConstIterator MatrixDeriv1ColConstIterator; - typedef typename DataTypes1::MatrixDeriv::RowIterator MatrixDeriv1RowIterator; - typedef typename DataTypes1::MatrixDeriv::ColIterator MatrixDeriv1ColIterator; - typedef typename DataTypes1::MatrixDeriv::RowType MatrixDerivRowType1; - - typedef typename DataTypes2::Real Real2; - typedef typename DataTypes2::VecCoord VecCoord2; - typedef typename DataTypes2::VecDeriv VecDeriv2; - typedef typename DataTypes2::Coord Coord2; - typedef typename DataTypes2::Deriv Deriv2; - typedef typename DataTypes2::MatrixDeriv MatrixDeriv2; - typedef typename DataTypes2::MatrixDeriv::RowConstIterator MatrixDeriv2RowConstIterator; - typedef typename DataTypes2::MatrixDeriv::ColConstIterator MatrixDeriv2ColConstIterator; - typedef typename DataTypes2::MatrixDeriv::RowIterator MatrixDeriv2RowIterator; - typedef typename DataTypes2::MatrixDeriv::ColIterator MatrixDeriv2ColIterator; - typedef typename DataTypes2::MatrixDeriv::RowType MatrixDerivRowType2; - -protected: - LMConstraint(); - LMConstraint( MechanicalState *dof1, MechanicalState *dof2); - - ~LMConstraint() override; -public: - void init() override; - - /// get Mechanical State 1 where the constraint will be expressed (can be a Mapped mechanical state) - BaseMechanicalState* getConstrainedMechModel1() const override {return constrainedObject1;} - /// get Mechanical State 2 where the constraint will be expressed (can be a Mapped mechanical state) - BaseMechanicalState* getConstrainedMechModel2() const override {return constrainedObject2;} - - /// get Mechanical State 1 where the constraint will be solved - BaseMechanicalState* getSimulatedMechModel1() const override {return simulatedObject1;} - /// get Mechanical State 2 where the constraint will be solved - BaseMechanicalState* getSimulatedMechModel2() const override {return simulatedObject2;} - - /// Pre-construction check method called by ObjectFactory. - template - static bool canCreate(T*& obj, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg) - { - if (arg->getAttribute("object1") || arg->getAttribute("object2")) - { - if (dynamic_cast*>(arg->findObject(arg->getAttribute("object1",".."))) == nullptr) { - arg->logError("Data attribute 'object1' must point the a valid mechanical state of datatype '" + std::string(DataTypes1::Name()) + "'."); - return false; - } - - if (dynamic_cast*>(arg->findObject(arg->getAttribute("object2",".."))) == nullptr) { - arg->logError("Data attribute 'object2' must point the a valid mechanical state of datatype '" + std::string(DataTypes2::Name()) + "'."); - return false; - } - } - else - { - if (dynamic_cast*>(context->getMechanicalState()) == nullptr) { - arg->logError("No mechanical state with the datatype '" + std::string(DataTypes1::Name()) + "' found in the context node."); - return false; - } - } - return sofa::core::objectmodel::BaseObject::canCreate(obj, context, arg); - } - - /// Construction method called by ObjectFactory. - template - static typename T::SPtr create(T* p0, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg) - { - typename T::SPtr obj = sofa::core::objectmodel::BaseObject::create(p0, context, arg); - - if (arg && (arg->getAttribute("object1") || arg->getAttribute("object2"))) - { - obj->constrainedObject1 = dynamic_cast*>(arg->findObject(arg->getAttribute("object1",".."))); - obj->constrainedObject2 = dynamic_cast*>(arg->findObject(arg->getAttribute("object2",".."))); - } - else if (context) - { - obj->constrainedObject1 = - obj->constrainedObject2 = - dynamic_cast*>(context->getMechanicalState()); - } - - return obj; - } - -protected: - MechanicalState *constrainedObject1; - MechanicalState *constrainedObject2; - - BaseMechanicalState *simulatedObject1; - BaseMechanicalState *simulatedObject2; -}; - -#if !defined(SOFA_CORE_BEHAVIOR_LMCONSTRAINT_CPP) -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; - -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; - -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; - -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; - -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; -extern template class LMCONSTRAINT_API LMConstraint; - - -#endif - -} // namespace behavior - -} // namespace core - -} // namespace sofa - -#endif diff --git a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.inl b/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.inl deleted file mode 100644 index d99f651121a..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraint.inl +++ /dev/null @@ -1,128 +0,0 @@ -/****************************************************************************** -* 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 * -******************************************************************************/ -#ifndef SOFA_CORE_BEHAVIOR_LMCONSTRAINT_INL -#define SOFA_CORE_BEHAVIOR_LMCONSTRAINT_INL - -#include -#include -#include - - -namespace sofa -{ - -namespace core -{ - -namespace behavior -{ - -template -LMConstraint::LMConstraint() - : LMConstraint(nullptr, nullptr) -{ -} - -template -LMConstraint::LMConstraint( MechanicalState *dof1, MechanicalState *dof2) - : constrainedObject1(dof1) - , constrainedObject2(dof2) - , simulatedObject1(dof1) - , simulatedObject2(dof2) -{ -} - - -template -LMConstraint::~LMConstraint() -{ -} - - -template -void LMConstraint::init() -{ - using sofa::core::objectmodel::BaseContext; - using sofa::core::objectmodel::BaseNode; - - BaseLMConstraint::init(); - - if (constrainedObject1 != nullptr && constrainedObject2 != nullptr) - { - //Constraint created by passing Mechanical State directly, need to find the name of the path to be able to save the scene eventually - - if (constrainedObject1->getContext() != getContext()) - { - BaseContext *context = nullptr; - BaseNode *currentNode = constrainedObject1->getContext()->toBaseNode(); - - std::string constrainedObject_name = currentNode->getPathName(); - if (context != nullptr) - this->pathObject1.setValue(constrainedObject_name); - } - - if (constrainedObject2->getContext() != getContext()) - { - BaseContext *context = nullptr; - BaseNode *currentNode = constrainedObject2->getContext()->toBaseNode(); - - std::string constrainedObject_name = currentNode->getPathName(); - if (context != nullptr) - this->pathObject2.setValue(constrainedObject_name); - } - - simulatedObject1 = constrainedObject1; - - while (simulatedObject1) - { - core::BaseMapping* mapping; - simulatedObject1->getContext()->get(mapping); - if (!mapping) - break; - if(!mapping->isMechanical()) - break; - simulatedObject1 = mapping->getMechFrom()[0]; - } - - simulatedObject2 = constrainedObject2; - - while (simulatedObject2) - { - core::BaseMapping* mapping; - simulatedObject2->getContext()->get(mapping); - if (!mapping) - break; - if(!mapping->isMechanical()) - break; - simulatedObject2 = mapping->getMechFrom()[0]; - } - } -} - - -} // namespace behavior - -} // namespace core - -} // namespace sofa - -#endif diff --git a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintDirectSolver.cpp b/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintDirectSolver.cpp deleted file mode 100644 index 523527eaec3..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintDirectSolver.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/****************************************************************************** -* 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 - - -namespace sofa::component::constraintset -{ - -LMConstraintDirectSolver::LMConstraintDirectSolver() - : solverAlgorithm(initData(&solverAlgorithm, "solverAlgorithm", "Algorithm used to solve the system W.Lambda=c")) -{ - //Add here other algo - sofa::helper::OptionsGroup algo(1,"SVD"); - solverAlgorithm.setValue(algo); -} - - -bool LMConstraintDirectSolver::buildSystem(const core::ConstraintParams* cParams, MultiVecId res1, MultiVecId res2) -{ - bool sucess = LMConstraintSolver::buildSystem(cParams, res1, res2); - - return sucess; -} - -bool LMConstraintDirectSolver::solveSystem(const core::ConstraintParams* cParams, MultiVecId res1, MultiVecId res2) -{ - //First, do n iterations of Gauss Seidel - bool success = LMConstraintSolver::solveSystem(cParams, res1, res2); - - - if (cParams->constOrder() != core::ConstraintParams::VEL) return success; - - - //Then process to a direct solution of the system - - //We need to find all the constraint related to contact - // 1. extract the information about the state of the contact and build the new L, L^T matrices - // 2. build the full system - // 3. solve - - - //------------------------------------------------------------------ - // extract the information about the state of the contact - //------------------------------------------------------------------ - - //************************************************************ -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printNode("AnalyseConstraints"); -#endif - const type::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints(); - - JacobianRows rowsL ; rowsL.reserve(numConstraint); - JacobianRows rowsLT; rowsLT.reserve(numConstraint); - type::vector< unsigned int > rightHandElements; - - analyseConstraints(LMConstraints, cParams->constOrder(), - rowsL, rowsLT, rightHandElements); - -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printCloseNode("AnalyseConstraints"); -#endif - if (rowsL.empty() || rowsLT.empty()) return success; - - - - - -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printNode("BuildFullSystem"); -#endif - //------------------------------------------------------------------ - // build c: right hand term - //------------------------------------------------------------------ - VectorEigen previousC(c); - //TODO: change newC by c - c=VectorEigen::Zero(rowsL.size()); - unsigned int idx=0; - for (type::vector::const_iterator it=rightHandElements.begin(); it!=rightHandElements.end(); ++it) - c[idx++]=previousC[*it]; - - //------------------------------------------------------------------ - // build the L and LT matrices - //------------------------------------------------------------------ - - - DofToMatrix LMatricesDirectSolver; - DofToMatrix LTMatricesDirectSolver; - for (DofToMatrix::iterator it=LMatrices.begin(); it!=LMatrices.end(); ++it) - { - //------------------------------------------------------------------ - const SparseMatrixEigen& matrix= it->second; - //Init the manipulator with the full matrix - linearalgebra::LMatrixManipulator manip; - manip.init(matrix); - - - //------------------------------------------------------------------ - SparseMatrixEigen L (rowsL.size(), matrix.cols()); - L.reserve(rowsL.size()*matrix.cols()); - manip.buildLMatrix(rowsL ,L); - L.finalize(); - LMatricesDirectSolver.insert (std::make_pair(it->first,L )); - - - - //------------------------------------------------------------------ - SparseMatrixEigen LT(rowsLT.size(), matrix.cols()); - LT.reserve(rowsLT.size()*matrix.cols()); - manip.buildLMatrix(rowsLT,LT); - LT.finalize(); - LTMatricesDirectSolver.insert(std::make_pair(it->first,LT)); - } - - - - //------------------------------------------------------------------ - // build the full system - //------------------------------------------------------------------ - const int rows=rowsL.size(); - const int cols=rowsLT.size(); - SparseColMajorMatrixEigen Wsparse(rows,cols); - buildLeftRectangularMatrix(invMassMatrix, LMatricesDirectSolver, LTMatricesDirectSolver, Wsparse,invMass_Ltrans); - - - //------------------------------------------------------------------ - // conversion from sparse to dense matrix - //------------------------------------------------------------------ - Lambda=VectorEigen::Zero(rows); - - W=MatrixEigen::Zero(rows,cols); - - - - SparseMatrixEigen Wresult(Wsparse); - for (int k=0; k solverSVD(W); - VectorEigen invSingularValues(solverSVD.singularValues()); - - for (int i=0; i &LMConstraints, core::ConstraintParams::ConstOrder order, - JacobianRows &rowsL,JacobianRows &rowsLT, type::vector< unsigned int > &rightHandElements) const -{ - //Iterate among all the Sofa LMConstraint - for (unsigned int componentConstraint=0; componentConstraint(constraint)) - { - const type::vector< sofa::core::behavior::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order); - //Iterate among all the contacts - for (type::vector< sofa::core::behavior::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup) - { - const sofa::core::behavior::ConstraintGroup* group=*itGroup; - const sofa::component::constraintset::ContactDescription& contact=contactDescriptor->getContactDescription(group); - - const unsigned int idxEquation=group->getConstraint(0).idx; - - switch(contact.state) - { - case VANISHING: - { - break; - } - case STICKING: - { - const unsigned int i=rowsL.size(); - rowsL.push_back(linearalgebra::LLineManipulator().addCombination(idxEquation )); - rowsL.push_back(linearalgebra::LLineManipulator().addCombination(idxEquation+1)); - rowsL.push_back(linearalgebra::LLineManipulator().addCombination(idxEquation+2)); - - //3 equations - rowsLT.push_back(rowsL[i ]); - rowsLT.push_back(rowsL[i+1]); - rowsLT.push_back(rowsL[i+2]); - - rightHandElements.push_back(idxEquation ); - rightHandElements.push_back(idxEquation+1); - rightHandElements.push_back(idxEquation+2); - break; - } - case SLIDING: - { - rowsL.push_back(linearalgebra::LLineManipulator().addCombination(idxEquation )); - rowsL.push_back(linearalgebra::LLineManipulator().addCombination(idxEquation+1)); - rowsL.push_back(linearalgebra::LLineManipulator().addCombination(idxEquation+2)); - - - //1 equation with the response force along the Coulomb friction cone - rowsLT.push_back(linearalgebra::LLineManipulator() - .addCombination(idxEquation ,contact.coeff[0]) - .addCombination(idxEquation+1,contact.coeff[1]) - .addCombination(idxEquation+2,contact.coeff[2])); - - rightHandElements.push_back(idxEquation ); - rightHandElements.push_back(idxEquation+1); - rightHandElements.push_back(idxEquation+2); - break; - } - } - } - } - else - { - //Non contact constraints: we add all the equations - const type::vector< sofa::core::behavior::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order); - for (type::vector< sofa::core::behavior::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup) - { - const sofa::core::behavior::ConstraintGroup* group=*itGroup; - std::pair< sofa::core::behavior::ConstraintGroup::EquationConstIterator,sofa::core::behavior::ConstraintGroup::EquationConstIterator> range=group->data(); - for ( sofa::core::behavior::ConstraintGroup::EquationConstIterator it=range.first; it!=range.second; ++it) - { - rowsL.push_back(linearalgebra::LLineManipulator().addCombination(it->idx)); - rowsLT.push_back(rowsL.back()); - rightHandElements.push_back(it->idx); - } - } - } - } -} - - - - -void LMConstraintDirectSolver::buildLeftRectangularMatrix(const DofToMatrix& invMassMatrix, - DofToMatrix& LMatrix, DofToMatrix& LTMatrix, - SparseColMajorMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const -{ - invMass_Ltrans.clear(); - for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs) - { - const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs; - const SparseMatrixEigen &invMass=invMassMatrix.find(dofs)->second; - const SparseMatrixEigen &L =LMatrix[dofs]; - const SparseMatrixEigen <=LTMatrix[dofs]; - - SparseMatrixEigen invMass_LT=invMass*LT.transpose(); - - invMass_Ltrans.insert(std::make_pair(dofs, invMass_LT)); - LeftMatrix += L*invMass_LT; - } -} -int LMConstraintDirectSolverClass = core::RegisterObject("A Direct Constraint Solver working specifically with LMConstraint based components") - .add< LMConstraintDirectSolver >(); - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintDirectSolver.h b/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintDirectSolver.h deleted file mode 100644 index 7f12746748a..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintDirectSolver.h +++ /dev/null @@ -1,58 +0,0 @@ -/****************************************************************************** -* 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 - -namespace sofa::component::constraintset -{ - -class LMCONSTRAINT_API LMConstraintDirectSolver : public LMConstraintSolver -{ - typedef Eigen::SparseMatrix SparseColMajorMatrixEigen; - typedef type::vector JacobianRows; - using MultiVecId = sofa::core::MultiVecId; - -public: - SOFA_CLASS(LMConstraintDirectSolver, LMConstraintSolver); -protected: - LMConstraintDirectSolver(); -public: - bool buildSystem(const core::ConstraintParams *, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; - bool solveSystem(const core::ConstraintParams *, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; - -protected: - - void analyseConstraints(const type::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints, core::ConstraintParams::ConstOrder order, - JacobianRows &rowsL,JacobianRows &rowsLT, type::vector< unsigned int > &rightHandElements) const; - - void buildLeftRectangularMatrix(const DofToMatrix& invMassMatrix, - DofToMatrix& LMatrix, DofToMatrix& LTMatrix, - SparseColMajorMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const; - - Data solverAlgorithm; ///< Algorithm used to solve the system W.Lambda=c -}; - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintSolver.cpp b/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintSolver.cpp deleted file mode 100644 index ff35af50aa7..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintSolver.cpp +++ /dev/null @@ -1,946 +0,0 @@ -/****************************************************************************** -* 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 -#include -#include - -#include - -#include -#include - -#include -#include - -#include -using sofa::simulation::mechanicalvisitor::MechanicalPropagateDxVisitor; - -#include -using sofa::simulation::mechanicalvisitor::MechanicalProjectJacobianMatrixVisitor; - -#include -using sofa::simulation::mechanicalvisitor::MechanicalProjectVelocityVisitor; - -#include -using sofa::simulation::mechanicalvisitor::MechanicalProjectPositionVisitor; - -#include -using sofa::simulation::mechanicalvisitor::MechanicalPropagateOnlyPositionVisitor; - -#include -using sofa::simulation::mechanicalvisitor::MechanicalPropagateOnlyVelocityVisitor; - -namespace sofa::component::constraintset -{ - -using linearsolver::FullVector; -using linearsolver::FullMatrix; - -LMConstraintSolver::LMConstraintSolver() - : constraintAcc( initData( &constraintAcc, false, "constraintAcc", "Constraint the acceleration")) - , constraintVel( initData( &constraintVel, true, "constraintVel", "Constraint the velocity")) - , constraintPos( initData( &constraintPos, true, "constraintPos", "Constraint the position")) - , numIterations( initData( &numIterations, 25u, "numIterations", "Number of iterations for Gauss-Seidel when solving the Constraints")) - , maxError( initData( &maxError, 0.0000001, "maxError", "threshold for the residue of the Gauss-Seidel algorithm")) - , graphGSError( initData(&graphGSError,"graphGSError","Graph of residuals at each iteration") ) - , traceKineticEnergy( initData( &traceKineticEnergy, false, "traceKineticEnergy", "Trace the evolution of the Kinetic Energy throughout the solution of the system")) - , graphKineticEnergy( initData(&graphKineticEnergy,"graphKineticEnergy","Graph of the kinetic energy of the system") ) - , LMConstraintVisitor( sofa::core::execparams::defaultInstance() ) -{ - graphGSError.setGroup("Statistics"); - graphGSError.setWidget("graph"); - - graphKineticEnergy.setGroup("Statistics"); - graphKineticEnergy.setWidget("graph"); - - traceKineticEnergy.setGroup("Statistics"); - this->f_listening.setValue(true); - - numIterations.setRequired(true); - maxError.setRequired(true); -} - -void LMConstraintSolver::init() -{ - sofa::core::behavior::ConstraintSolver::init(); - - type::vector< core::behavior::BaseConstraintCorrection* > listConstraintCorrection; - getContext()->get(&listConstraintCorrection, core::objectmodel::BaseContext::SearchDown); - for (auto* constraintCorrection : listConstraintCorrection) - { - core::behavior::BaseMechanicalState* constrainedDof; - constraintCorrection->getContext()->get(constrainedDof); - if (constrainedDof) - { - constraintCorrections.insert(std::make_pair(constrainedDof, constraintCorrection)); - constraintCorrection->addConstraintSolver(this); - } - } - - graphKineticEnergy.setDisplayed(traceKineticEnergy.getValue()); -} -void LMConstraintSolver::removeConstraintCorrection(core::behavior::BaseConstraintCorrection * /*s*/) -{ - //TODO: remove the pair containing s from constraintCorrections -} - -void LMConstraintSolver::convertSparseToDense(const SparseMatrixEigen& sparseM, MatrixEigen& denseM) const -{ - denseM=MatrixEigen::Zero(sparseM.rows(), sparseM.cols()); - - for (int k=0; k c; - this->getContext()->get(&c, core::objectmodel::BaseContext::SearchDown); - for (unsigned int i=0; iisCorrectionComputedWithSimulatedDOF(order)) - { - needPriorPropagation=true; - msg_info() << "Propagating the State because of "<< c[i]->getName() ; - break; - } - } - } - return needPriorPropagation; -} - -bool LMConstraintSolver::prepareStates(const core::ConstraintParams *cparams, MultiVecId id, MultiVecId /*res2*/) -{ - //Get the matrices through mappings - //************************************************************ - // Update the State of the Mapped dofs // - //************************************************************ - core::MechanicalParams mparams; -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::TRACE_ARGUMENT arg; -#endif - using core::behavior::BaseLMConstraint ; - LMConstraintVisitor.clear(); - - VecId vid = id.getDefaultId(); - - orderState=cparams->constOrder(); - LMConstraintVisitor.setMultiVecId(id); - if (orderState==core::ConstraintParams::ACC) - { - if (!constraintAcc.getValue()) return false; - if (needPriorStatePropagation(orderState)) - { - MechanicalPropagateDxVisitor propagateState(&mparams, core::VecDerivId(vid), false); - propagateState.execute(this->getContext()); - } - // calling writeConstraintEquations - LMConstraintVisitor.setOrder(orderState); - LMConstraintVisitor.setTags(getTags()).execute(this->getContext()); - - msg_info() << "prepareStates for accelerations"; - - MechanicalProjectJacobianMatrixVisitor(&mparams).execute(this->getContext()); -#ifdef SOFA_DUMP_VISITOR_INFO - arg.push_back(std::make_pair("Order", "Acceleration")); -#endif - } - else if (orderState==core::ConstraintParams::VEL) - { - if (!constraintVel.getValue()) return false; - - msg_info() << "prepareStates for velocities"; - - MechanicalProjectVelocityVisitor projectState(&mparams, this->getContext()->getTime(), core::VecDerivId(vid)); - projectState.execute(this->getContext()); - if (needPriorStatePropagation(orderState)) - { - MechanicalPropagateOnlyVelocityVisitor propagateState(&mparams, 0.0, core::VecDerivId(vid)); - propagateState.execute(this->getContext()); - } - - // calling writeConstraintEquations - LMConstraintVisitor.setOrder(orderState); - LMConstraintVisitor.setTags(getTags()).execute(this->getContext()); - - MechanicalProjectJacobianMatrixVisitor(&mparams).execute(this->getContext()); - -#ifdef SOFA_DUMP_VISITOR_INFO - arg.push_back(std::make_pair("Order", "Velocity")); -#endif - - } - else - { - if (!constraintPos.getValue()) return false; - msg_info() << "prepareStates for positions"; - - MechanicalProjectPositionVisitor projectPos(&mparams, this->getContext()->getTime(), core::VecCoordId(vid)); - projectPos.execute(this->getContext()); - if (needPriorStatePropagation(orderState)) - { - MechanicalPropagateOnlyPositionVisitor propagateState(&mparams, 0.0, core::VecCoordId(vid)); - propagateState.execute(this->getContext()); - } - - // calling writeConstraintEquations - LMConstraintVisitor.setOrder(orderState); - LMConstraintVisitor.setTags(getTags()).execute(this->getContext()); - -#ifdef SOFA_DUMP_VISITOR_INFO - arg.push_back(std::make_pair("Order", "Position")); -#endif - } - - //************************************************************ - // Find the number of constraints // - //************************************************************ - numConstraint=0; - const type::vector< BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints(); - for (unsigned int mat=0; matgetNumConstraint(orderState); - msg_info() << "LMConstraintSolver::prepareStates, constraint " << LMConstraints[mat]->getName() - << " between "<< LMConstraints[mat]->getSimulatedMechModel1()->getName() - << " and " << LMConstraints[mat]->getSimulatedMechModel2()->getName() - <<", add "<< LMConstraints[mat]->getNumConstraint(orderState) - << " constraint for order "<< orderState ; - } - if (numConstraint == 0) - { - return false; //Nothing to solve - } - - - if (orderState==core::ConstraintParams::POS) - { - sofa::helper::AdvancedTimer::valSet("numConstraintsPosition", numConstraint); - msg_info() << "Applying the constraint on the position"; - } - else if (orderState==core::ConstraintParams::VEL) - { - sofa::helper::AdvancedTimer::valSet("numConstraintsVelocity", numConstraint); - msg_info() << "Applying the constraint on the velocity" ; - } - else if (orderState==core::ConstraintParams::ACC) - { - sofa::helper::AdvancedTimer::valSet("numConstraintsAcceleration", numConstraint); - msg_info() << "Applying the constraint on the acceleration" ; - } - else msg_warning() << "Order Not recognized " << orderState ; - - setDofs.clear(); - dofUsed.clear(); - invMass_Ltrans.clear(); - return true; -} - -bool LMConstraintSolver::buildSystem(const core::ConstraintParams *cParams, MultiVecId id, MultiVecId /*res2*/) -{ -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printNode("SystemCreation"); -#endif - - sofa::helper::AdvancedTimer::stepBegin("SolveConstraints " + id.getName() + " BuildSystem Prepare"); - const type::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints(); - //Informations to build the matrices - //Dofs to be constrained - for (unsigned int mat=0; matgetNumConstraint(cParams->constOrder())) - { - setDofs.insert(constraint->getSimulatedMechModel1()); - setDofs.insert(constraint->getSimulatedMechModel2()); - } - } - for (SetDof::iterator it=setDofs.begin(); it!=setDofs.end();) - { - SetDof::iterator currentIt=it; - ++it; - - if (!(*currentIt)->getContext()->getMass()) - { - setDofs.erase(currentIt); - } -#ifdef SOFA_DUMP_VISITOR_INFO - else if (sofa::simulation::Visitor::IsExportStateVectorEnabled()) - { - sofa::simulation::Visitor::printNode("Input_"+(*currentIt)->getName()); - sofa::simulation::Visitor::printVector(*currentIt, id.getDefaultId()); - sofa::simulation::Visitor::printCloseNode("Input_"+(*currentIt)->getName()); - } -#endif - } - sofa::helper::AdvancedTimer::stepEnd ("SolveConstraints " + id.getName() + " BuildSystem Prepare"); - - - //Store the indices used in order to speed up the correction propagation - //************************************************************ - // Build the Right Hand Term - //************************************************************ - sofa::helper::AdvancedTimer::stepBegin("SolveConstraints " + id.getName() + " BuildSystem C"); - c = VectorEigen::Zero((int)numConstraint); - buildRightHandTerm(LMConstraints, c, id, orderState); - sofa::helper::AdvancedTimer::stepEnd("SolveConstraints " + id.getName() + " BuildSystem C"); - - - //************************************************************ - // Build M^-1 - //************************************************************ - sofa::helper::AdvancedTimer::stepBegin("SolveConstraints " + id.getName() + " BuildSystem M^-1"); - buildInverseMassMatrices(setDofs, invMassMatrix); - sofa::helper::AdvancedTimer::stepEnd("SolveConstraints " + id.getName() + " BuildSystem M^-1"); - - //************************************************************ - //Building L - //************************************************************ - sofa::helper::AdvancedTimer::stepBegin("SolveConstraints " + id.getName() + " BuildSystem L"); - LMatrices.clear(); - //Init matrices to the good size - for (SetDof::iterator it=setDofs.begin(); it!=setDofs.end(); ++it) - { - const core::behavior::BaseMechanicalState* dofs=*it; - SparseMatrixEigen L(numConstraint,dofs->getSize()*dofs->getDerivDimension()); - L.reserve(numConstraint*(1+dofs->getSize()));//TODO: give a better estimation of non-zero coefficients - LMatrices.insert(std::make_pair(dofs, L)); - } - buildLMatrices(orderState, LMConstraints, LMatrices, dofUsed); - - //Remove empty L Matrices - for (DofToMatrix::iterator it=LMatrices.begin(); it!=LMatrices.end();) - { - SparseMatrixEigen& matrix= it->second; - matrix.finalize(); - DofToMatrix::iterator itCurrent=it; - ++it; - if (!matrix.nonZeros()) //Empty Matrix: act as an obstacle - { - invMassMatrix.erase(itCurrent->first); - setDofs.erase(const_cast(itCurrent->first)); - LMatrices.erase(itCurrent); - } - - } - sofa::helper::AdvancedTimer::stepEnd("SolveConstraints " + id.getName() + " BuildSystem L"); - - - //************************************************************ - // Building W=L0.M0^-1.L0^T + L1.M1^-1.L1^T + ... and M^-1.L^T - //************************************************************ - //Store the matrix M^-1.L^T for each dof in order to apply the modification of the state - sofa::helper::AdvancedTimer::stepBegin("SolveConstraints " + id.getName() + " BuildSystem W"); - SparseMatrixEigen WEi((int)numConstraint,(int)numConstraint); - buildLeftMatrix(invMassMatrix, LMatrices,WEi, invMass_Ltrans); - sofa::helper::AdvancedTimer::stepEnd("SolveConstraints " + id.getName() + " BuildSystem W"); - - sofa::helper::AdvancedTimer::stepBegin("SolveConstraints " + id.getName() + " BuildSystem conversionW"); - //Convert the Sparse Matrix AEi into a Dense Matrix-> faster access to elements, and possilibity to use a direct LU solution - convertSparseToDense(WEi,W); - - sofa::helper::AdvancedTimer::stepEnd("SolveConstraints " + id.getName() + " BuildSystem conversionW"); -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printCloseNode("SystemCreation"); -#endif - return true; -} - -bool LMConstraintSolver::solveSystem(const core::ConstraintParams* cParams, MultiVecId id, MultiVecId /*res2*/) -{ -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printNode("SystemConstraintSolution"); -#endif - - //************************************************************ - // Solving the System using Eigen2 - //************************************************************ - msg_info() << "W= L0.M0^-1.L0^T + L1.M1^-1.L1^T + ...: " << msgendl - << W << msgendl - << "for a constraint: " << msgendl - << c ; - - const type::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints(); - - //"Cold" start - Lambda=VectorEigen::Zero(numConstraint); - bool solutionFound=solveConstraintSystemUsingGaussSeidel(id, cParams->constOrder(), LMConstraints, - W, c, Lambda); -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printCloseNode("SystemConstraintSolution"); -#endif - return solutionFound; -} - -bool LMConstraintSolver::applyCorrection(const core::ConstraintParams* cparams, MultiVecId id, MultiVecId /*res2*/) -{ - //************************************************************ - // Constraint Correction - //************************************************************ -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printNode("SystemCorrection"); -#endif - //************************************************************ - // Updating the state vectors - // get the displacement. deltaState = M^-1.L^T.lambda : lambda being the solution of the system - for (SetDof::iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs) - { - sofa::core::behavior::BaseMechanicalState* dofs=*itDofs; - bool updateVelocities=!constraintVel.getValue(); - VecId v = id.getId(dofs); - constraintStateCorrection(v, cparams->constOrder(), updateVelocities,invMass_Ltrans[dofs] , Lambda, dofUsed[dofs], dofs); - -#ifdef SOFA_DUMP_VISITOR_INFO - if (sofa::simulation::Visitor::IsExportStateVectorEnabled()) - { - sofa::simulation::Visitor::printNode("Output_"+dofs->getName()); - sofa::simulation::Visitor::printVector(dofs, v); - sofa::simulation::Visitor::printCloseNode("Output_"+dofs->getName()); - } -#endif - } - -#ifdef SOFA_DUMP_VISITOR_INFO - sofa::simulation::Visitor::printCloseNode("SystemCorrection"); -#endif - return true; -} - -//----------------------------------------------------------------------------------------------// -// Specific method to build the matrices - -void LMConstraintSolver::buildLeftMatrix(const DofToMatrix& invMassMatrix, DofToMatrix& LMatrix, SparseMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const -{ - for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs) - { - const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs; - const SparseMatrixEigen &invMass=invMassMatrix.find(dofs)->second; - const SparseMatrixEigen &L=LMatrix[dofs]; - - msg_info() << "Matrix L for " << dofs->getName() << "\n" << L << msgendl - << "Matrix M-1 for " << dofs->getName() << "\n" << invMass ; - - const SparseMatrixEigen &invM_LTrans=invMass*L.transpose(); - invMass_Ltrans.insert( std::make_pair(dofs,invM_LTrans) ); - LeftMatrix += L*invM_LTrans; - } -} - -void LMConstraintSolver::buildLMatrices( ConstOrder Order, - const type::vector< core::behavior::BaseLMConstraint* > &LMConstraints, - DofToMatrix &LMatrices, - DofToMask &dofUsed) const -{ - unsigned constraintOffset=0; - //We Take one by one the constraint, and write their equations in the corresponding matrix L - for (unsigned int mat=0; matgetSimulatedMechModel1(); - const core::behavior::BaseMechanicalState *dof2=constraint->getSimulatedMechModel2(); - - //Get the entries in the Vector of constraints corresponding to the constraint equations - std::list< unsigned int > equationsUsed; - constraint->getEquationsUsed(Order, equationsUsed); - - if (equationsUsed.empty()) continue; - - DofToMatrix::iterator itL1=LMatrices.find(dof1); - if (itL1 != LMatrices.end()) - { - buildLMatrix(itL1->first, equationsUsed, constraintOffset, itL1->second, dofUsed[dof1]); - } - - DofToMatrix::iterator itL2=LMatrices.find(dof2); - if (dof1 != dof2 && itL2 != LMatrices.end()) - { - buildLMatrix(itL2->first, equationsUsed,constraintOffset, itL2->second,dofUsed[dof2]); - } - constraintOffset += equationsUsed.size(); - } -} - -void LMConstraintSolver::buildInverseMassMatrices( const SetDof &setDofs, DofToMatrix& invMassMatrices) -{ - for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs) - { - const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs; - const unsigned int dimensionDofs=dofs->getDerivDimension(); - - - //Verify if the Big M^-1 matrix has not already been computed and stored in memory - DofToMatrix::iterator mFound = invMassMatrices.find(dofs); - bool needToConstructMassMatrix = true; - if (mFound != invMassMatrices.end()) - { - //WARNING HACK! we should find a way to know when the Mass changed: in some case the number of Dof can be the same, but the mass changed - if ((int) (dofs->getSize()*dimensionDofs) != mFound->second.rows()) - needToConstructMassMatrix=true; - else - needToConstructMassMatrix=false; - } - - //************************************************************ - //Building M^-1 - //Called only ONCE! - core::behavior::BaseMass *mass=dofs->getContext()->getMass(); - - if (needToConstructMassMatrix) - { - SparseMatrixEigen invMass(dofs->getSize()*dimensionDofs, dofs->getSize()*dimensionDofs); - invMass.reserve(dofs->getSize()*dimensionDofs*dimensionDofs); - - DofToConstraintCorrection::iterator constraintCorrectionFound=constraintCorrections.find(dofs); - if (constraintCorrectionFound != constraintCorrections.end()) - { - buildInverseMassMatrix(dofs, constraintCorrectionFound->second, invMass); - } - else - { - buildInverseMassMatrix(dofs, mass, invMass); - } - invMass.finalize(); - //Store the matrix in memory - if (mFound != invMassMatrices.end()) - invMassMatrices[dofs]=invMass; - else - invMassMatrices.insert( std::make_pair(dofs,invMass) ); - } - } -} - -void LMConstraintSolver::buildInverseMassMatrix( const sofa::core::behavior::BaseMechanicalState* /*mstate*/, const core::behavior::BaseConstraintCorrection* constraintCorrection, SparseMatrixEigen& invMass) const -{ - //Get the Full Matrix from the constraint correction - FullMatrix computationInvM; - constraintCorrection->getComplianceMatrix(&computationInvM); - //Then convert it into a Sparse Matrix: as it is done only at the init, or when topological changes occur, this should not be a burden for the framerate - for (Index i=0; igetDerivDimension(); - //Build M, using blocks of [ dimensionDofs x dimensionDofs ] given for each particle - //computationM is a block corresponding the mass matrix of a particle - FullMatrix computationM(dimensionDofs, dimensionDofs); - MatrixEigen invMEigen((int)dimensionDofs,(int)dimensionDofs); - - for (size_t i=0; igetSize(); ++i) - { - mass->getElementMass(i,&computationM); - - //Translate the FullMatrix into a Eigen Matrix to invert it - MatrixEigen mapMEigen=Eigen::Map(computationM[0],(int)computationM.rowSize(),(int)computationM.colSize()); - invMEigen=mapMEigen.inverse(); - - //Store into the sparse matrix the block corresponding to the inverse of the mass matrix of a particle - for (unsigned int r=0; r &idxEquations, unsigned int constraintOffset, - SparseMatrixEigen& L, std::set< unsigned int > &dofUsed) const -{ - const unsigned int dimensionDofs=dof->getDerivDimension(); - typedef core::behavior::BaseMechanicalState::ConstraintBlock ConstraintBlock; - //Get blocks of values from the Mechanical States - std::list< ConstraintBlock > blocks=dof->constraintBlocks( idxEquations ); - - - //Fill the matrices - const unsigned int numEquations=idxEquations.size(); - - for (unsigned int eq=0; eq::const_iterator itBlock=blocks.begin(); itBlock!=blocks.end(); ++itBlock) - { - const ConstraintBlock &b=(*itBlock); - const defaulttype::BaseMatrix &m=b.getMatrix(); - const unsigned int column=b.getColumn()*dimensionDofs; - - for (Index j=0; j::iterator itBlock=blocks.begin(); itBlock!=blocks.end(); ++itBlock) - { - delete itBlock->getMatrix(); - } -} - -void LMConstraintSolver::buildRightHandTerm( const type::vector< core::behavior::BaseLMConstraint* > &LMConstraints, VectorEigen &c, - MultiVecId /*id*/, ConstOrder Order) const -{ - - FullVector c_fullvector(c.data(), (Index)c.rows()); - for (unsigned int mat=0; matgetConstraintViolation(&c_fullvector, Order); -} - -bool LMConstraintSolver::solveConstraintSystemUsingGaussSeidel( MultiVecId id, ConstOrder Order, - const type::vector< core::behavior::BaseLMConstraint* > &LMConstraints, - const MatrixEigen &W, const VectorEigen &c, VectorEigen &Lambda) -{ - msg_info() << "Using Gauss-Seidel solution" ; - - std::string orderName; - switch (Order) - { - case core::ConstraintParams::ACC : - orderName="Acceleration"; - break; - case core::ConstraintParams::VEL : - orderName="Velocity"; - break; - case core::ConstraintParams::POS : - case core::ConstraintParams::POS_AND_VEL : - orderName="Position"; - break; - } - - const SReal invNormC=1.0/c.norm(); - - type::vector &vError=(*graphGSError.beginEdit())["Error "+ orderName]; - - vError.push_back(c.sum()); - graphGSError.endEdit(); - - VectorEigen LambdaPrevious=Lambda; - - //Store the invalid block of the W matrix - std::set< int > emptyBlock; - type::vector< std::pair > constraintToBlock; - type::vector< MatrixEigen> constraintInvWBlock; - type::vector< MatrixEigen> constraintWBlock; - - - //Preparation Step: - { - //Iterate on all the Constraint components - unsigned int idxConstraint=0; - for (unsigned int componentConstraint=0; componentConstraint &constraintOrder=constraint->getConstraintsOrder(Order); - - unsigned int numConstraintToProcess=0; - for (unsigned int constraintEntry=0; constraintEntrygetNumConstraint(); - - const MatrixEigen &wConstraint=W.block(idxConstraint,idxConstraint,numConstraintToProcess, numConstraintToProcess); - if (wConstraint.diagonal().isZero(1e-15)) emptyBlock.insert(idxConstraint); - constraintWBlock.push_back(wConstraint); - constraintInvWBlock.push_back(wConstraint.inverse()); - - const VectorEigen &cb=c.block(idxConstraint , 0, - numConstraintToProcess, 1); - const MatrixEigen &wb=W.block(idxConstraint , 0, - numConstraintToProcess, W.cols()); - constraintToBlock.push_back(std::make_pair(wb,cb)); - } - } - } - //-- Initialization of X, solution of the system - bool continueIteration=true; - bool correctionDone=true; - unsigned int iteration=0; - - const unsigned int numIterationsGS=numIterations.getValue(); - const unsigned int numLMConstraintComponents=LMConstraints.size(); - - for (; iteration < numIterationsGS && continueIteration && correctionDone; ++iteration) - { - unsigned int idxConstraint=0; - VectorEigen LambdaPreviousIteration; - continueIteration=false; - correctionDone=false; - unsigned int idxBlocks=0; - //Iterate on all the Constraint components - - for (unsigned int componentConstraint=0; componentConstraint &constraintOrder=constraint->getConstraintsOrder(Order); - - unsigned int numConstraintToProcess=0; - for (unsigned int constraintEntry=0; constraintEntrygetNumConstraint(); - - //Invalid constraints: due to projective constraints, or constraint expressed for Obstacle objects: we just ignore them - if (emptyBlock.find(idxConstraint) != emptyBlock.end()) continue; - - LambdaPreviousIteration = Lambda.block(idxConstraint,0,numConstraintToProcess,1); - //Set to Zero sigma for the current set of constraints - Lambda.block(idxConstraint,0,numConstraintToProcess,1).setZero(); - - - const MatrixEigen &Wblock =constraintWBlock[idxBlocks]; - const MatrixEigen &invWblock=constraintInvWBlock[idxBlocks]; - - const std::pair &blocks=constraintToBlock[idxBlocks]; - const VectorEigen &cb=blocks.second; - const MatrixEigen &wb=blocks.first; - - //Compute Sigma - VectorEigen sigma = cb; sigma.noalias() = sigma - (wb * Lambda); - - VectorEigen newLambda; newLambda.noalias() = invWblock*sigma; - constraint->LagrangeMultiplierEvaluation(Wblock.data(),sigma.data(), newLambda.data(), - constraintOrder[constraintEntry]); - - //**************************************************************** - if (!constraintOrder[constraintEntry]->isActive()) - { - Lambda.block(idxConstraint,0,numConstraintToProcess,1).setZero(); - - msg_info() << "["<< iteration << "/" << numIterations.getValue() <<"] ###Deactivated###" - << (LambdaPreviousIteration - Lambda.block(idxConstraint,0,numConstraintToProcess,1)).sum() - << " for system n°" << idxConstraint << "/" << W.cols() << " of " << numConstraintToProcess << " equations " - << " between " << constraint->getSimulatedMechModel1()->getName() << " and " << constraint->getSimulatedMechModel2()->getName() ; - - correctionDone |= ( (LambdaPreviousIteration - Lambda.block(idxConstraint,0,numConstraintToProcess,1)).sum() != 0); - } - else - { - Lambda.block(idxConstraint,0,numConstraintToProcess,1)=newLambda; - - msg_info() <<"["<< iteration << "/" << numIterations.getValue() <<"] " - << (LambdaPreviousIteration - Lambda.block(idxConstraint,0,numConstraintToProcess,1)).sum() - << " for system n°" << idxConstraint << "/" << W.cols() << " of " << numConstraintToProcess << " equations " - << " between " << constraint->getSimulatedMechModel1()->getName() << " and " << constraint->getSimulatedMechModel2()->getName() ; - - correctionDone |= ( (LambdaPreviousIteration - Lambda.block(idxConstraint,0,numConstraintToProcess,1)).sum() != 0); - } - } - } - - if (Order == core::ConstraintParams::VEL && traceKineticEnergy.getValue()) - { - if (iteration == 0) - { - graphKineticEnergy.beginEdit()->clear(); - graphKineticEnergy.endEdit(); - } - VectorEigen LambdaSave=Lambda; - Lambda.noalias() = Lambda - LambdaPrevious; - computeKineticEnergy(id); - Lambda = LambdaSave; - LambdaPrevious=Lambda; - } - - - const SReal residual=(c-W*Lambda).norm()*invNormC; - vError.push_back( residual ); - graphGSError.endEdit(); - continueIteration = (residual > maxError.getValue()); - - msg_info() << "ITERATION " << iteration << " ENDED: Residual for iteration " << iteration << " = " << residual ; - } - - msg_info_when(iteration == numIterations.getValue()) << "no convergence in Gauss-Seidel for " << orderName; - - msg_info() << "Gauss-Seidel done in " << iteration << " iterations "; - - if (Order == core::ConstraintParams::VEL && traceKineticEnergy.getValue()) return false; - return true; -} - -void LMConstraintSolver::constraintStateCorrection(VecId id, core::ConstraintParams::ConstOrder order, - bool isPositionChangesUpdateVelocity, - const SparseMatrixEigen &invM_Ltrans, - const VectorEigen &c, - const std::set< unsigned int > &dofUsed, - sofa::core::behavior::BaseMechanicalState* dofs) const -{ - - //Correct Dof - // operation: M0^-1.L0^T.lambda -> A - - VectorEigen A; - /* This seems very redundant, but the SparseTimeDenseProduct seems buggy when the dense rhs - and sparse lhs uses different storage order. This is always the case with a sparse row major - and a dense vector (col major otherwise compile time assert )*/ - //Eigen::SparseMatrix invM_Ltrans_colMajor = Eigen::SparseMatrix(invM_Ltrans); - - A.noalias() = invM_Ltrans*c; - - msg_info() << "M^-1.L^T " << "\n" << invM_Ltrans << msgendl - << "Lambda " << dofs->getName() << "\n" << c << msgendl - << "Correction " << dofs->getName() << "\n" << A ; - - const unsigned int dimensionDofs=dofs->getDerivDimension(); - - //In case of position correction, we need to update the velocities - if (order==core::ConstraintParams::POS) - { - //Detect Rigid Bodies - if (dofs->getCoordDimension() == 7 && dofs->getDerivDimension() == 6) - { - VectorEigen Acorrection=VectorEigen::Zero(dofs->getSize()*(3+4)); - //We have to transform the Euler Rotations into a quaternion - unsigned int offset=0; - - for (int l=0; l q=type::Quat::createQuaterFromEuler(type::Vector3(A(l+3),A(l+4),A(l+5))); - - Acorrection(l+3+offset)=q[0]; - Acorrection(l+4+offset)=q[1]; - Acorrection(l+5+offset)=q[2]; - Acorrection(l+6+offset)=q[3]; - } - offset=0; - FullVector v(Acorrection.data(), (Index)Acorrection.rows()); - - msg_info() << "Lambda Corrected for Rigid " << "\n" << Acorrection ; - - dofs->addFromBaseVectorSameSize(id,&v,offset ); - - } - else - { - std::set< unsigned int >::const_iterator it; - for (it=dofUsed.begin(); it!=dofUsed.end(); ++it) - { - unsigned int offset=(*it); - FullVector v(&(A.data()[offset*dimensionDofs]),dimensionDofs); - dofs->addFromBaseVectorDifferentSize(id,&v,offset ); - } - } - - if (isPositionChangesUpdateVelocity) - { - const double h=1.0/getContext()->getDt(); - - std::set< unsigned int >::const_iterator it; - for (it=dofUsed.begin(); it!=dofUsed.end(); ++it) - { - unsigned int offset=(*it); - FullVector v(&(A.data()[offset*dimensionDofs]),dimensionDofs); - for (unsigned int i=0; iaddFromBaseVectorDifferentSize(id,&v,offset ); - } - } - - } - else - { - std::set< unsigned int >::const_iterator it; - for (it=dofUsed.begin(); it!=dofUsed.end(); ++it) - { - unsigned int offset=(*it); - FullVector v(&(A.data()[offset*dimensionDofs]),dimensionDofs); - dofs->addFromBaseVectorDifferentSize(id,&v,offset ); - } - - } -} - -void LMConstraintSolver::computeKineticEnergy(MultiVecId id) -{ - type::vector &vError=(*graphKineticEnergy.beginEdit())["KineticEnergy"]; - - core::ConstraintParams cparam; - cparam.setOrder(core::ConstraintParams::VEL); - - applyCorrection(&cparam, id); - - double kineticEnergy=0; - for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs) - { - const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs; - const core::behavior::BaseMass *mass=dofs->getContext()->getMass(); - if (mass) kineticEnergy += mass->getKineticEnergy(); - } - vError.push_back(kineticEnergy); - graphKineticEnergy.endEdit(); -} - -void LMConstraintSolver::handleEvent(core::objectmodel::Event *e) -{ - if (sofa::simulation::AnimateBeginEvent::checkEventType(e)) - { - graphGSError.beginEdit()->clear(); - graphGSError.endEdit(); - } -} - - - -int LMConstraintSolverClass = core::RegisterObject("A Constraint Solver working specifically with LMConstraint based components") - .add< LMConstraintSolver >(); - -} //namespace sofa::component::constraintset \ No newline at end of file diff --git a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintSolver.h b/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintSolver.h deleted file mode 100644 index 9564ee3f94a..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/LMConstraintSolver.h +++ /dev/null @@ -1,169 +0,0 @@ -/****************************************************************************** -* 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::constraintset -{ - -class LMCONSTRAINT_API LMConstraintSolver : public sofa::core::behavior::ConstraintSolver -{ -protected: - typedef sofa::core::VecId VecId; - typedef sofa::core::MultiVecId MultiVecId; - typedef sofa::core::ConstraintParams::ConstOrder ConstOrder; - - typedef Eigen::Matrix MatrixEigen; - typedef linearalgebra::VectorEigen VectorEigen; - typedef defaulttype::BaseVector::Index Index; - typedef linearalgebra::SparseMatrixEigen SparseMatrixEigen; - typedef linearalgebra::SparseVectorEigen SparseVectorEigen; - - typedef std::set< sofa::core::behavior::BaseMechanicalState* > SetDof; - typedef std::map< const sofa::core::behavior::BaseMechanicalState *, SparseMatrixEigen > DofToMatrix; - typedef std::map< const sofa::core::behavior::BaseMechanicalState *, std::set > DofToMask; - typedef std::map< const sofa::core::behavior::BaseMechanicalState *, core::behavior::BaseConstraintCorrection* > DofToConstraintCorrection; - -public: - SOFA_CLASS(LMConstraintSolver, sofa::core::behavior::ConstraintSolver); -protected: - LMConstraintSolver(); -public: - void init() override; - void reinit() override {graphKineticEnergy.setDisplayed(traceKineticEnergy.getValue());}; - - void removeConstraintCorrection(core::behavior::BaseConstraintCorrection *s) override; - - bool prepareStates(const core::ConstraintParams *, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; - bool buildSystem(const core::ConstraintParams *, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; - bool solveSystem(const core::ConstraintParams *, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; - bool applyCorrection(const core::ConstraintParams *, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; - - void handleEvent( core::objectmodel::Event *e) override; - - - - Data constraintAcc; ///< Constraint the acceleration - Data constraintVel; ///< Constraint the velocity - Data constraintPos; ///< Constraint the position - Data numIterations; ///< Number of iterations for Gauss-Seidel when solving the Constraints - Data maxError; ///< threshold for the residue of the Gauss-Seidel algorithm - mutable Data > > graphGSError; ///< Graph of residuals at each iteration - Data< bool > traceKineticEnergy; ///< Trace the evolution of the Kinetic Energy throughout the solution of the system - mutable Data > > graphKineticEnergy; ///< Graph of the kinetic energy of the system - - - - template - std::string printDimension(const T& m) - { - std::ostringstream out; - out << "(" << m.rows() << "," << m.cols() << ")"; - return out.str(); - } - - void convertSparseToDense(const SparseMatrixEigen& sparseM, MatrixEigen& out) const; -protected: - /// Explore the graph, looking for LMConstraints: each LMConstraint can tell if they need State Propagation in order to compute the right hand term of the system - virtual bool needPriorStatePropagation(core::ConstraintParams::ConstOrder order) const; - - /// Construct the Right hand term of the system - virtual void buildRightHandTerm ( const type::vector< core::behavior::BaseLMConstraint* > &LMConstraints, - VectorEigen &c, MultiVecId, ConstOrder Order ) const; - /// Construct the Inverse of the mass matrix for a set of Dofs - virtual void buildInverseMassMatrices( const SetDof &setDofs, - DofToMatrix& invMassMatrices); - /// Construct the L matrices: write the constraint equations, and use dofUsed to remember the particles used in order to speed up the constraint correction - virtual void buildLMatrices ( ConstOrder Order, const type::vector< core::behavior::BaseLMConstraint* > &LMConstraints, - DofToMatrix &LMatrices, DofToMask &dofUsed) const; - /// Construct the Left Matrix A=sum_i(L_i.M^{-1}_i.L_i^T), store the matrices M^{-1}_i.L_i^T in order to compute later the constraint correction to apply - virtual void buildLeftMatrix ( const DofToMatrix& invMassMatrices, - DofToMatrix& LMatrices, SparseMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const; - /// Solve the System using a projective Gauss-Seidel algorithm: compute the Lagrange Multipliers Lambda - virtual bool solveConstraintSystemUsingGaussSeidel(MultiVecId id, ConstOrder Order, - const type::vector< core::behavior::BaseLMConstraint* > &LMConstraints, - const MatrixEigen &W, - const VectorEigen &c, - VectorEigen &Lambda); - - /// Compute Kinetic Energy - virtual void computeKineticEnergy(MultiVecId id); - - /** Apply the correction to the state corresponding - * @param id nature of the constraint, and correction to apply - * @param dof MechanicalState to correct - * @param invM_Ltrans matrix M^-1.L^T to apply the correction from the independant dofs through the mapping - * @param c correction vector - * @param propageVelocityChange need to propagate the correction done to the velocity for the position - **/ - virtual void constraintStateCorrection(VecId id, core::ConstraintParams::ConstOrder order, - bool isPositionChangesUpdateVelocity, - const SparseMatrixEigen &invM_Ltrans, - const VectorEigen &Lambda, - const std::set< unsigned int > &dofUsed, - sofa::core::behavior::BaseMechanicalState* dof) const; - - - /// - virtual void buildLMatrix ( const sofa::core::behavior::BaseMechanicalState *dof, - const std::list &idxEquations,unsigned int constraintOffset, - SparseMatrixEigen& L, std::set< unsigned int > &dofUsed ) const; - virtual void buildInverseMassMatrix( const sofa::core::behavior::BaseMechanicalState* mstate, - const core::behavior::BaseConstraintCorrection* constraintCorrection, - SparseMatrixEigen& matrix) const; - virtual void buildInverseMassMatrix( const sofa::core::behavior::BaseMechanicalState* mstate, - const core::behavior::BaseMass* mass, - SparseMatrixEigen& matrix) const; - - core::ConstraintParams::ConstOrder orderState; - unsigned int numConstraint; - - //Variables used to do the computation - sofa::simulation::MechanicalWriteLMConstraint LMConstraintVisitor; - SetDof setDofs; - DofToMask dofUsed; - DofToMatrix invMass_Ltrans; - DofToMatrix LMatrices; - - MatrixEigen W; - VectorEigen c; - VectorEigen Lambda; - - //Persitent datas - DofToMatrix invMassMatrix; - DofToConstraintCorrection constraintCorrections; - -}; - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/MechanicalWriteLMConstraint.cpp b/applications/plugins/LMConstraint/src/LMConstraint/MechanicalWriteLMConstraint.cpp deleted file mode 100644 index 29fc3f55c7d..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/MechanicalWriteLMConstraint.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/****************************************************************************** -* 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 LMCONSTRAINT_MECHANICALWRITELMCONSTRAINT_CPP -#include - -namespace sofa -{ - -namespace simulation -{ -using namespace sofa::core; - - -Visitor::Result MechanicalWriteLMConstraint::fwdConstraintSet(simulation::Node* /*node*/, core::behavior::BaseConstraintSet* c) -{ - if (core::behavior::BaseLMConstraint* LMc=dynamic_cast(c)) - { - LMc->writeConstraintEquations(offset, id, order); - datasC.push_back(LMc); - } - return RESULT_CONTINUE; -} - -std::string MechanicalWriteLMConstraint::getInfos() const -{ - std::string name; - if (order == core::ConstraintParams::ACC) - name= "["+sofa::core::VecId::dx().getName()+"]"; - else if (order == core::ConstraintParams::VEL) - name= "["+sofa::core::VecId::velocity().getName()+"]"; - else if (order == core::ConstraintParams::POS) - name= "["+sofa::core::VecId::position().getName()+"]"; - return name; -} - - - -} // namespace simulation - -} // namespace sofa - diff --git a/applications/plugins/LMConstraint/src/LMConstraint/MechanicalWriteLMConstraint.h b/applications/plugins/LMConstraint/src/LMConstraint/MechanicalWriteLMConstraint.h deleted file mode 100644 index f4211e66d08..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/MechanicalWriteLMConstraint.h +++ /dev/null @@ -1,82 +0,0 @@ -/****************************************************************************** -* 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 - -namespace sofa -{ - -namespace simulation -{ - - -class LMCONSTRAINT_API MechanicalWriteLMConstraint : public sofa::simulation::BaseMechanicalVisitor -{ -public: - MechanicalWriteLMConstraint(const sofa::core::ExecParams * params) - : BaseMechanicalVisitor(params) - , offset(0) - {} - - Result fwdConstraintSet(simulation::Node* /*node*/, core::behavior::BaseConstraintSet* c) override; - // This visitor must go through all mechanical mappings, even if isMechanical flag is disabled - bool stopAtMechanicalMapping(simulation::Node* /*node*/, core::BaseMapping* /*map*/) override - { - return false; // !map->isMechanical(); - } - - /// Return a class name for this visitor - /// Only used for debugging / profiling purposes - const char* getClassName() const override { return "MechanicalWriteLMConstraint"; } - virtual std::string getInfos() const override ; - - virtual void clear() {datasC.clear(); offset=0;} - virtual const std::vector< core::behavior::BaseLMConstraint *> &getConstraints() const {return datasC;} - virtual unsigned int numConstraint() {return static_cast(datasC.size());} - - virtual void setMultiVecId(core::MultiVecId i) {id=i;} - core::MultiVecId getMultiVecId() const { return id; } - - - virtual void setOrder(core::ConstraintParams::ConstOrder i) {order=i;} - core::ConstraintParams::ConstOrder getOrder() const { return order; } - - bool isThreadSafe() const override - { - return false; - } - -protected: - unsigned int offset; - sofa::core::ConstraintParams::ConstOrder order; - core::MultiVecId id; - type::vector< core::behavior::BaseLMConstraint *> datasC; - -}; - - -} // namespace simulation - -} // namespace sofa - diff --git a/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.cpp b/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.cpp deleted file mode 100644 index 42da01674fa..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/****************************************************************************** -* 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 LMCONSTRAINT_PRECOMPUTEDLMCONSTRAINTCORRECTION_CPP - -#include -#include - -namespace sofa::component::constraintset -{ - - -using namespace sofa::defaulttype; - -int PrecomputedLMConstraintCorrectionClass = core::RegisterObject("Component computing constraint forces within a simulated body using the compliance method.") - .add< PrecomputedLMConstraintCorrection >() - .add< PrecomputedLMConstraintCorrection >() - .add< PrecomputedLMConstraintCorrection >() - - ; - -template class LMCONSTRAINT_API PrecomputedLMConstraintCorrection; -template class LMCONSTRAINT_API PrecomputedLMConstraintCorrection; -template class LMCONSTRAINT_API PrecomputedLMConstraintCorrection; - - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.h b/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.h deleted file mode 100644 index 7a1615095b0..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.h +++ /dev/null @@ -1,85 +0,0 @@ -/****************************************************************************** -* 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 - - -namespace sofa::component::constraintset -{ - -/** - * \brief Component computing constraint forces within a simulated body using the compliance method. - */ -template -class PrecomputedLMConstraintCorrection : public sofa::component::constraintset::PrecomputedConstraintCorrection< TDataTypes > -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(PrecomputedLMConstraintCorrection,TDataTypes), SOFA_TEMPLATE(sofa::component::constraintset::PrecomputedConstraintCorrection, TDataTypes)); - - typedef TDataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - 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 sofa::core::behavior::ConstraintCorrection< TDataTypes > Inherit; - - typedef typename Coord::value_type Real; - typedef sofa::type::MatNoInit<3, 3, Real> Transformation; - - Data m_rotations; - Data m_restRotations; - - Data recompute; ///< if true, always recompute the compliance - Data debugViewFrameScale; ///< Scale on computed node's frame - sofa::core::objectmodel::DataFileName f_fileCompliance; ///< Precomputed compliance matrix data file - Data fileDir; ///< If not empty, the compliance will be saved in this repertory - -protected: - PrecomputedLMConstraintCorrection(sofa::core::behavior::MechanicalState *mm = nullptr) - { - SOFA_UNUSED(mm); - }; - -public: - void bwdInit() override; -}; - - - - - -#if !defined(LMCONSTRAINT_PRECOMPUTEDLMCONSTRAINTCORRECTION_CPP) -extern template class LMCONSTRAINT_API PrecomputedLMConstraintCorrection; -extern template class LMCONSTRAINT_API PrecomputedLMConstraintCorrection; -extern template class LMCONSTRAINT_API PrecomputedLMConstraintCorrection; - -#endif - - -} //namespace sofa::component::constraintset diff --git a/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.inl b/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.inl deleted file mode 100644 index 3a37ec95fa3..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/PrecomputedLMConstraintCorrection.inl +++ /dev/null @@ -1,259 +0,0 @@ -/****************************************************************************** -* 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 - - -namespace sofa::component::constraintset -{ - -template -void PrecomputedLMConstraintCorrection::bwdInit() -{ - Inherit::init(); - - const VecDeriv& v0 = this->mstate->read(core::ConstVecDerivId::velocity())->getValue(); - - this->nbNodes = v0.size(); - - if (this->nbNodes == 0) - { - msg_error() << "No degree of freedom" ; - return; - } - - this->dof_on_node = v0[0].size(); - - this->nbRows = this->nbNodes * this->dof_on_node; - this->nbCols = this->nbNodes * this->dof_on_node; - - double dt = this->getContext()->getDt(); - - this->invName = f_fileCompliance.getFullPath().empty() ? this->buildFileName() : f_fileCompliance.getFullPath(); - - if (!this->loadCompliance(this->invName)) - { - msg_info() << "Compliance being built"; - - // Buffer Allocation - this->invM->data = new Real[this->nbRows * this->nbCols]; - - // for the intial computation, the gravity has to be put at 0 - const sofa::type::Vec3d gravity = this->getContext()->getGravity(); - - const sofa::type::Vec3d gravity_zero(0.0,0.0,0.0); - this->getContext()->setGravity(gravity_zero); - - sofa::component::odesolver::EulerImplicitSolver* eulerSolver; - sofa::component::linearsolver::CGLinearSolver< sofa::component::linearsolver::GraphScatteredMatrix, sofa::component::linearsolver::GraphScatteredVector >* cgLinearSolver; - core::behavior::LinearSolver* linearSolver; - - this->getContext()->get(eulerSolver); - this->getContext()->get(cgLinearSolver); - this->getContext()->get(linearSolver); - - simulation::Node *solvernode = nullptr; - - if (eulerSolver && cgLinearSolver) - { - msg_info() << "use EulerImplicitSolver & CGLinearSolver" ; - solvernode = (simulation::Node*)eulerSolver->getContext(); - } - else if (eulerSolver && linearSolver) - { - msg_info() << "use EulerImplicitSolver & LinearSolver"; - solvernode = (simulation::Node*)eulerSolver->getContext(); - } - else if(eulerSolver) - { - msg_info() << "use EulerImplicitSolver"; - solvernode = (simulation::Node*)eulerSolver->getContext(); - } - else - { - msg_error() << "PrecomputedContactCorrection must be associated with EulerImplicitSolver+LinearSolver for the precomputation\nNo Precomputation" ; - return; - } - - type::vector< sofa::component::constraintset::LMConstraintSolver* > listLMConstraintSolver; - solvernode->get< sofa::component::constraintset::LMConstraintSolver >(&listLMConstraintSolver, core::objectmodel::BaseContext::SearchDown); - type::vector< sofa::component::constraintset::ConstraintActivation > listConstraintActivation(listLMConstraintSolver.size()); - for (unsigned int i=0; iconstraintAcc.getValue(); - listLMConstraintSolver[i]->constraintAcc.setValue(false); - - listConstraintActivation[i].vel=listLMConstraintSolver[i]->constraintVel.getValue(); - listLMConstraintSolver[i]->constraintVel.setValue(false); - - listConstraintActivation[i].pos=listLMConstraintSolver[i]->constraintPos.getValue(); - listLMConstraintSolver[i]->constraintPos.setValue(false); - } - - // Change the solver parameters - double buf_tolerance = 0, buf_threshold = 0; - int buf_maxIter = 0; - - if (cgLinearSolver) - { - buf_tolerance = (double) cgLinearSolver->d_tolerance.getValue(); - buf_maxIter = (int) cgLinearSolver->d_maxIter.getValue(); - buf_threshold = (double) cgLinearSolver->d_smallDenominatorThreshold.getValue(); - - cgLinearSolver->d_tolerance.setValue(1e-20); - cgLinearSolver->d_maxIter.setValue(5000); - cgLinearSolver->d_smallDenominatorThreshold.setValue(1e-35); - } - - - helper::ReadAccessor< Data< VecCoord > > rposData = *this->mstate->read(core::ConstVecCoordId::position()); - const VecCoord prev_pos = rposData.ref(); - - helper::WriteAccessor< Data< VecDeriv > > velocityData = *this->mstate->write(core::VecDerivId::velocity()); - VecDeriv& velocity = velocityData.wref(); - const VecDeriv prev_velocity = velocity; - - helper::WriteAccessor< Data< VecDeriv > > forceData = *this->mstate->write(core::VecDerivId::externalForce()); - VecDeriv& force = forceData.wref(); - force.clear(); - force.resize(this->nbNodes); - - /// christian : it seems necessary to called the integration one time for initialization - /// (avoid to have a line of 0 at the top of the matrix) - if (eulerSolver) - { - eulerSolver->solve(core::execparams::defaultInstance(), dt, core::VecCoordId::position(), core::VecDerivId::velocity()); - } - - Deriv unitary_force; - - std::stringstream tmpStr; - for (unsigned int f = 0; f < this->nbNodes; f++) - { - tmpStr.precision(2); - tmpStr << "Precomputing constraint correction : " << std::fixed << (float)f / (float)this->nbNodes * 100.0f << " % " << '\xd'; - - for (unsigned int i = 0; i < this->dof_on_node; i++) - { - unitary_force.clear(); - unitary_force[i] = 1.0; - - force[f] = unitary_force; - - // Reset positions and velocities - velocity.clear(); - velocity.resize(this->nbNodes); - - // Actualize ref to the position vector ; it seems it is changed at every eulerSolver->solve() - helper::WriteOnlyAccessor< Data< VecCoord > > wposData = *this->mstate->write(core::VecCoordId::position()); - VecCoord& pos = wposData.wref(); - - for (unsigned int n = 0; n < this->nbNodes; n++) - pos[n] = prev_pos[n]; - - double fact = 1.0 / dt; // christian : it is not a compliance... but an admittance that is computed ! - - if (eulerSolver) - { - fact *= eulerSolver->getPositionIntegrationFactor(); // here, we compute a compliance - - eulerSolver->solve(core::execparams::defaultInstance(), dt, core::VecCoordId::position(), core::VecDerivId::velocity()); - - if (linearSolver) - linearSolver->freezeSystemMatrix(); // do not recompute the matrix for the rest of the precomputation - } - - for (unsigned int v = 0; v < this->nbNodes; v++) - { - for (unsigned int j = 0; j < this->dof_on_node; j++) - { - this->invM->data[(v * this->dof_on_node + j) * this->nbCols + (f * this->dof_on_node + i) ] = (Real)(fact * velocity[v][j]); - } - } - } - - unitary_force.clear(); - force[f] = unitary_force; - } - msg_info() << tmpStr.str(); - - // Do not recompute the matrix for the rest of the precomputation - if (linearSolver) - linearSolver->freezeSystemMatrix(); - - this->saveCompliance(this->invName); - - // Restore gravity - this->getContext()->setGravity(gravity); - - // Restore linear solver parameters - if (cgLinearSolver) - { - cgLinearSolver->d_tolerance.setValue(buf_tolerance); - cgLinearSolver->d_maxIter.setValue(buf_maxIter); - cgLinearSolver->d_smallDenominatorThreshold.setValue(buf_threshold); - } - - // Retore velocity - for (unsigned int i = 0; i < velocity.size(); i++) - velocity[i] = prev_velocity[i]; - - helper::WriteOnlyAccessor< Data< VecCoord > > wposData = *this->mstate->write(core::VecCoordId::position()); - VecCoord& pos = wposData.wref(); - - // Restore position - for (unsigned int i = 0; i < pos.size(); i++) - pos[i] = prev_pos[i]; - - for (unsigned int i=0; iconstraintAcc.setValue(listConstraintActivation[i].acc); - listLMConstraintSolver[i]->constraintVel.setValue(listConstraintActivation[i].vel); - listLMConstraintSolver[i]->constraintPos.setValue(listConstraintActivation[i].pos); - } - } - - this->appCompliance = this->invM->data; - - // Optimisation for the computation of W - this->_indexNodeSparseCompliance.resize(v0.size()); - - // Print 400 first row and column of the matrix - if (this->notMuted()) - { - msg_info() << "Matrix compliance : this->nbCols = " << this->nbCols << " this->nbRows =" << this->nbRows; - - for (unsigned int i = 0; i < 20 && i < this->nbCols; i++) - { - for (unsigned int j = 0; j < 20 && j < this->nbCols; j++) - { - msg_info() << " \t " << this->appCompliance[j*this->nbCols + i]; - } - } - - } -} - - -} diff --git a/applications/plugins/LMConstraint/src/LMConstraint/TetrahedronBarycentricDistanceLMConstraintContact.cpp b/applications/plugins/LMConstraint/src/LMConstraint/TetrahedronBarycentricDistanceLMConstraintContact.cpp deleted file mode 100644 index ca82227863c..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/TetrahedronBarycentricDistanceLMConstraintContact.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/****************************************************************************** -* 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 - -using namespace sofa::defaulttype; -using namespace sofa::core::collision; - -namespace sofa -{ - -namespace component -{ - -namespace collision -{ - -Creator> > TetrahedronSphereDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > TetrahedronPointDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > TetrahedronLineDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator> > TetrahedronTriangleDistanceLMConstraintContactClass("distanceLMConstraint",true); -Creator > TetrahedronTetrahedronDistanceLMConstraintContactClass("distanceLMConstraint",true); - -} // namespace collision - -} // namespace component - -} // namespace sofa - diff --git a/applications/plugins/LMConstraint/src/LMConstraint/config.h.in b/applications/plugins/LMConstraint/src/LMConstraint/config.h.in deleted file mode 100644 index 600de781cd9..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/config.h.in +++ /dev/null @@ -1,34 +0,0 @@ -/****************************************************************************** -* 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 - -#define LMCONSTRAINT_VERSION @PROJECT_VERSION@ - -#ifdef SOFA_BUILD_LMCONSTRAINT -# define SOFA_TARGET @PROJECT_NAME@ -# define LMCONSTRAINT_API SOFA_EXPORT_DYNAMIC_LIBRARY -#else -# define LMCONSTRAINT_API SOFA_IMPORT_DYNAMIC_LIBRARY -#endif - diff --git a/applications/plugins/LMConstraint/src/LMConstraint/initLMConstraint.cpp b/applications/plugins/LMConstraint/src/LMConstraint/initLMConstraint.cpp deleted file mode 100644 index aad58cba154..00000000000 --- a/applications/plugins/LMConstraint/src/LMConstraint/initLMConstraint.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/****************************************************************************** -* 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 -using sofa::core::ObjectFactory; - -extern "C" { - LMCONSTRAINT_API void initExternalModule(); - LMCONSTRAINT_API const char* getModuleName(); - LMCONSTRAINT_API const char* getModuleVersion(); - LMCONSTRAINT_API const char* getModuleLicense(); - LMCONSTRAINT_API const char* getModuleDescription(); - LMCONSTRAINT_API const char* getModuleComponentList(); -} - -void initExternalModule() -{ - static bool first = true; - if (first) - { - first = false; - } -} - -const char* getModuleName() -{ - return sofa_tostring(SOFA_TARGET); -} - -const char* getModuleVersion() -{ - return sofa_tostring(LMCONSTRAINT_VERSION); -} - -const char* getModuleLicense() -{ - return "LGPL"; -} - -const char* getModuleDescription() -{ - return "Previous implementation of the Lagrange Multiplier constraints"; -} - -const char* getModuleComponentList() -{ - /// string containing the names of the classes provided by the plugin - static std::string classes = ObjectFactory::getInstance()->listClassesFromTarget(sofa_tostring(SOFA_TARGET)); - return classes.c_str(); -} - diff --git a/applications/plugins/LMConstraint/tutorials/houseOfCards/CMakeLists.txt b/applications/plugins/LMConstraint/tutorials/houseOfCards/CMakeLists.txt deleted file mode 100644 index 301fbd8ae3d..00000000000 --- a/applications/plugins/LMConstraint/tutorials/houseOfCards/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -cmake_minimum_required(VERSION 3.12) -project(houseOfCards) - -find_package(SofaGui REQUIRED) -find_package(SofaComponentAll REQUIRED) -find_package(SceneCreator REQUIRED) - -add_executable(${PROJECT_NAME} houseOfCards.cpp) -target_link_libraries(${PROJECT_NAME} SofaGui SofaComponentAll SceneCreator) diff --git a/applications/plugins/LMConstraint/tutorials/houseOfCards/houseOfCards.cpp b/applications/plugins/LMConstraint/tutorials/houseOfCards/houseOfCards.cpp deleted file mode 100644 index 215524c6fc6..00000000000 --- a/applications/plugins/LMConstraint/tutorials/houseOfCards/houseOfCards.cpp +++ /dev/null @@ -1,281 +0,0 @@ -/****************************************************************************** -* 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 General Public License as published by the Free * -* Software Foundation; either version 2 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 General Public License for * -* more details. * -* * -* You should have received a copy of the GNU 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 -#include - -#include -#include - -#include - -#include -#include -#include -#include - -// --------------------------------------------------------------------- -// --- -// --------------------------------------------------------------------- -using namespace sofa::simulation; -using namespace sofa::component::container; -using namespace sofa::component::topology; -typedef sofa::component::linearsolver::CGLinearSolver< sofa::component::linearsolver::GraphScatteredMatrix, sofa::component::linearsolver::GraphScatteredVector> CGLinearSolverGraph; - -const std::string colors[7]= {"red","green","blue","cyan","magenta","yellow","white"}; - -SReal convertDegreeToRadian(const SReal& angle) -{ - return angle*M_PI/180.0; -} - -Node::SPtr createCard(Node::SPtr parent, const Coord3& position, const Coord3& rotation) -{ - const std::string visualModel="mesh/card.obj"; - const std::string collisionModel="mesh/cardCollision.obj"; - const std::string inertiaMatrix="BehaviorModels/card.rigid"; - static int colorIdx=0; - - std::vector modelTypes; - modelTypes.push_back("TriangleCollisionModel"); - modelTypes.push_back("LineCollisionModel"); - modelTypes.push_back("PointCollisionModel"); - - Node::SPtr card = sofa::modeling::createEulerSolverNode(parent,"Rigid","Implicit"); - - sofa::component::odesolver::EulerImplicitSolver *odeSolver; card->get(odeSolver); - odeSolver->f_rayleighStiffness.setValue(0.1); - odeSolver->f_rayleighMass.setValue(0.1); - - CGLinearSolverGraph *cgLinearSolver; card->get(cgLinearSolver); - cgLinearSolver->d_maxIter.setValue(15); - cgLinearSolver->d_tolerance.setValue(1e-5); - cgLinearSolver->d_smallDenominatorThreshold.setValue(1e-5); - - - sofa::component::constraintset::LMConstraintSolver::SPtr constraintSolver = sofa::core::objectmodel::New(); - constraintSolver->constraintVel.setValue(true); - constraintSolver->constraintPos.setValue(true); - constraintSolver->numIterations.setValue(25); - - card->addObject(constraintSolver); - - MechanicalObjectRigid3::SPtr dofRigid = sofa::core::objectmodel::New(); dofRigid->setName("Rigid Object"); - dofRigid->setTranslation(position[0],position[1],position[2]); - dofRigid->setRotation(rotation[0],rotation[1],rotation[2]); - card->addObject(dofRigid); - - UniformMassRigid3::SPtr uniMassRigid = sofa::core::objectmodel::New(); - uniMassRigid->setTotalMass(0.5); - uniMassRigid->setFileMass(inertiaMatrix); - card->addObject(uniMassRigid); - - //Node VISUAL - Node::SPtr RigidVisualNode = sofa::modeling::createVisualNodeRigid(card, dofRigid.get(), visualModel,colors[(colorIdx++)%7]); - - //Node COLLISION - Node::SPtr RigidCollisionNode = sofa::modeling::createCollisionNodeRigid(card, dofRigid.get(),collisionModel,modelTypes); - - return card; -} - -std::pair create2Cards(Node::SPtr parent, const Coord3& globalPosition, SReal distanceInBetween=SReal(0.1), SReal angle=SReal(15.0)) -{ - //We assume the card has a 2 unit length, centered in (0,0,0) - const SReal displacement=sin(convertDegreeToRadian(angle)); - const Coord3 separation=Coord3(displacement+distanceInBetween,0,0); - - //************************************ - //Left Rigid Card - const Coord3 positionLeft=globalPosition + separation; - const Coord3 rotationLeft(0,0,angle); - Node::SPtr leftCard = createCard(parent, positionLeft, rotationLeft); - - //************************************ - //Right Rigid Card - const Coord3 positionRight=globalPosition - separation; - const Coord3 rotationRight(0,0,-angle); - Node::SPtr rightCard = createCard(parent, positionRight, rotationRight); - - return std::make_pair(leftCard, rightCard); -} - -Node::SPtr createHouseOfCards(Node::SPtr root, unsigned int size, SReal distanceInBetween=SReal(0.1), SReal angle=SReal(15.0)) -{ - - //Elements of the scene - //------------------------------------ - Node::SPtr houseOfCards = root; - - - //Space between two levels of the house of cards - const SReal space=0.75; - //Size of a card - const SReal sizeCard=2; - //overlap of the cards - const SReal factor=0.95; - const SReal distanceH=sizeCard*factor; - const SReal distanceV=sizeCard*cos(convertDegreeToRadian(angle))+space; - - for (unsigned int i=0; i &cards=create2Cards(houseOfCards, position,distanceInBetween, angle); - std::ostringstream out; - out << "Card["<< i << "|" << j << "]"; - cards.first->setName("Left"+out.str()); - cards.second->setName("Right"+out.str()); - } - - //Create the support for the cards - const Coord3 initPosition(0,sizeCard*0.5,0); - const Coord3 supportRotation(0,0,90); - for (unsigned int j=0; jsetName(out.str()); - } - } - - - - return root; -} - - - -int main(int argc, char** argv) -{ - sofa::simulation::tree::init(); - sofa::component::initSofaComponentAll(); - sofa::gui::initMain(); - - unsigned int sizeHouseOfCards=4; - SReal angle=20.0; - SReal distanceInBetween=0.1; - SReal friction=0.8; - SReal contactDistance=0.03; - std::string gui = ""; - - - std::string gui_help = "choose the UI ("; - gui_help += sofa::gui::GUIManager::ListSupportedGUI('|'); - gui_help += ")"; - - sofa::helper::parse("This is a SOFA application. Here are the command line arguments") - .option(&sizeHouseOfCards,'l',"level","number of level of the house of cards") - .option(&angle,'a',"angle","angle formed by two cards") - .option(&distanceInBetween,'d',"distance","distance between two cards") - .option(&friction,'f',"friction","friction coeff") - .option(&contactDistance,'c',"contactDistance","contact distance") - .option(&gui,'g',"gui",gui_help.c_str()) - (argc,argv); - - - sofa::simulation::setSimulation(new sofa::simulation::tree::TreeSimulation()); - - // The graph root node - Node::SPtr root = sofa::modeling::createRootWithCollisionPipeline("distanceLMConstraint"); - root->setGravity( Coord3(0,-10,0) ); - root->setDt(0.001); - - sofa::component::collision::MinProximityIntersection *intersection; - root->get(intersection, sofa::core::objectmodel::BaseContext::SearchDown); - intersection->alarmDistance.setValue(contactDistance); - intersection->contactDistance.setValue(contactDistance*0.5); - - //************************************ - //Floor - Node::SPtr torusFixed = sofa::modeling::createObstacle(root.get(),"mesh/floor.obj", "mesh/floor.obj", "gray"); - - //Add the objects - createHouseOfCards(root.get(),sizeHouseOfCards,distanceInBetween, angle); - - - const SReal contactFriction=sqrt(friction); - sofa::type::vector< sofa::core::CollisionModel* > listCollisionModels; - root->getTreeObjects(&listCollisionModels); - for (unsigned int i=0; isetContactFriction(contactFriction); - root->setAnimate(false); - - //======================================= - // Export the scene to file - const std::string fileName="HouseOfCards.xml"; - sofa::simulation::getSimulation()->exportXML(root.get(),fileName.c_str()); - - //======================================= - // Destroy created scene: step needed, as I can't get rid of the locales (the mass can't init correctly as 0.1 is not considered as a floating point). - sofa::simulation::DeleteVisitor deleteScene(sofa::core::ExecParams::defaultInstance() ); - root->execute(deleteScene); - root.reset(); - - //======================================= - // Create the GUI - if (int err=sofa::gui::GUIManager::Init(argv[0],gui.c_str())) - return err; - - if (int err=sofa::gui::GUIManager::createGUI(NULL)) - return err; - - sofa::gui::GUIManager::SetDimension(800,600); - //======================================= - // Load the Scene - - sofa::simulation::Node::SPtr groot = sofa::core::objectmodel::SPtr_dynamic_cast( sofa::simulation::getSimulation()->load(fileName.c_str())); - if (groot==NULL) - { - groot = sofa::simulation::getSimulation()->createNewGraph(""); - } - - sofa::simulation::getSimulation()->init(groot.get()); - sofa::gui::GUIManager::SetScene(groot,fileName.c_str()); - - - //======================================= - // Run the main loop - if (int err=sofa::gui::GUIManager::MainLoop(groot,fileName.c_str())) - return err; - Node* ggroot = sofa::gui::GUIManager::CurrentSimulation(); - - - if (ggroot!=NULL) sofa::simulation::getSimulation()->unload(groot); - - sofa::simulation::tree::cleanup(); - return 0; -}