From 6132a52d637c2ee6b986e38ac1b92fa0ad9031ef Mon Sep 17 00:00:00 2001 From: Hadar Segal Date: Wed, 19 Mar 2025 08:51:06 +0200 Subject: [PATCH 1/3] collision detection exclusion pairs --- Simulation/CollisionDetection.h | 2 +- Simulation/DistanceFieldCollisionDetection.cpp | 17 +++++++++++++++++ Simulation/DistanceFieldCollisionDetection.h | 17 ++++++++++++++++- 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/Simulation/CollisionDetection.h b/Simulation/CollisionDetection.h index 33d1b112..55d296e0 100644 --- a/Simulation/CollisionDetection.h +++ b/Simulation/CollisionDetection.h @@ -67,7 +67,7 @@ namespace PBD void init(); - void cleanup(); + virtual void cleanup(); Real getTolerance() const { return m_tolerance; } void setTolerance(Real val) { m_tolerance = val; } diff --git a/Simulation/DistanceFieldCollisionDetection.cpp b/Simulation/DistanceFieldCollisionDetection.cpp index 9764d0c0..26bb81d5 100644 --- a/Simulation/DistanceFieldCollisionDetection.cpp +++ b/Simulation/DistanceFieldCollisionDetection.cpp @@ -23,6 +23,18 @@ DistanceFieldCollisionDetection::~DistanceFieldCollisionDetection() { } +void DistanceFieldCollisionDetection::cleanup() +{ + CollisionDetection::cleanup(); + m_exclusionPairs.clear(); +} + +void DistanceFieldCollisionDetection::addExclusionPair(unsigned int bodyIndex1, unsigned int bodyIndex2) +{ + auto hashCode = getPairHash(bodyIndex1, bodyIndex2); + m_exclusionPairs.insert(hashCode); +} + void DistanceFieldCollisionDetection::collisionDetection(SimulationModel &model) { model.resetContacts(); @@ -101,6 +113,10 @@ void DistanceFieldCollisionDetection::collisionDetection(SimulationModel &model) CollisionDetection::CollisionObject *co1 = m_collisionObjects[coPair.first]; CollisionDetection::CollisionObject *co2 = m_collisionObjects[coPair.second]; + auto hashCode = getPairHash(coPair.first, coPair.second); + if (m_exclusionPairs.find(hashCode) != m_exclusionPairs.end()) + continue; + if (((co2->m_bodyType != CollisionDetection::CollisionObject::RigidBodyCollisionObjectType) && (co2->m_bodyType != CollisionDetection::CollisionObject::TetModelCollisionObjectType)) || !isDistanceFieldCollisionObject(co1) || @@ -173,6 +189,7 @@ void DistanceFieldCollisionDetection::collisionDetection(SimulationModel &model) } } + for (unsigned int i = 0; i < contacts_mt.size(); i++) { for (unsigned int j = 0; j < contacts_mt[i].size(); j++) diff --git a/Simulation/DistanceFieldCollisionDetection.h b/Simulation/DistanceFieldCollisionDetection.h index 3cf44db5..e7efd815 100644 --- a/Simulation/DistanceFieldCollisionDetection.h +++ b/Simulation/DistanceFieldCollisionDetection.h @@ -5,6 +5,7 @@ #include "Simulation/CollisionDetection.h" #include "AABB.h" #include "BoundingSphereHierarchy.h" +#include namespace PBD { @@ -120,9 +121,11 @@ namespace PBD unsigned int m_elementIndex2; Vector3r m_bary1; Vector3r m_bary2; - }; + }; protected: + std::unordered_set m_exclusionPairs; + void collisionDetectionRigidBodies(RigidBody *rb1, DistanceFieldCollisionObject *co1, RigidBody *rb2, DistanceFieldCollisionObject *co2, const Real restitutionCoeff, const Real frictionCoeff , std::vector > &contacts_mt @@ -142,6 +145,14 @@ namespace PBD bool findRefTetAt(const ParticleData &pd, TetModel *tm, const DistanceFieldCollisionDetection::DistanceFieldCollisionObject *co, const Vector3r &X, unsigned int &tetIndex, Vector3r &barycentricCoordinates); + inline unsigned long long getPairHash(uint32_t bodyIndex1, uint32_t bodyIndex2) + { + // decode two 32 bits into a single 64 bit + if (bodyIndex2 < bodyIndex1) + std::swap(bodyIndex1, bodyIndex2); + return bodyIndex1 + (((unsigned long long)bodyIndex2) << 32); + } + public: DistanceFieldCollisionDetection(); @@ -151,6 +162,8 @@ namespace PBD virtual bool isDistanceFieldCollisionObject(CollisionObject *co) const; + void addExclusionPair(unsigned int bodyIndex1, unsigned int bodyIndex2); + void addCollisionBox(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector3r &box, const bool testMesh = true, const bool invertSDF = false); void addCollisionSphere(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Real radius, const bool testMesh = true, const bool invertSDF = false); void addCollisionTorus(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector2r &radii, const bool testMesh = true, const bool invertSDF = false); @@ -165,6 +178,8 @@ namespace PBD * @param dim (radius, height) of cylinder */ void addCollisionCylinder(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector2r &dim, const bool testMesh = true, const bool invertSDF = false); + + void cleanup() override; }; } From 6d6d4f21e999b68e3e416854ac8c48fd6916ca46 Mon Sep 17 00:00:00 2001 From: Hadar Segal Date: Wed, 19 Mar 2025 08:51:38 +0200 Subject: [PATCH 2/3] add new demo --- Demos/StiffRodsDemos/CMakeLists.txt | 30 +++ Demos/StiffRodsDemos/PlectonemesDemo.cpp | 294 +++++++++++++++++++++++ 2 files changed, 324 insertions(+) create mode 100644 Demos/StiffRodsDemos/PlectonemesDemo.cpp diff --git a/Demos/StiffRodsDemos/CMakeLists.txt b/Demos/StiffRodsDemos/CMakeLists.txt index 56c005e0..79b22c46 100644 --- a/Demos/StiffRodsDemos/CMakeLists.txt +++ b/Demos/StiffRodsDemos/CMakeLists.txt @@ -85,6 +85,36 @@ set_target_properties(DirectPositionBasedSolverForStiffRodsDemo PROPERTIES MINSI add_dependencies(DirectPositionBasedSolverForStiffRodsDemo ${SIMULATION_DEPENDENCIES}) target_link_libraries(DirectPositionBasedSolverForStiffRodsDemo ${SIMULATION_LINK_LIBRARIES}) +#### plectonemesDemo + +add_executable(PlectonemesDemo + PlectonemesDemo.cpp + + ../Common/LogWindow.cpp + ../Common/LogWindow.h + ../Common/Simulator_GUI_imgui.cpp + ../Common/Simulator_GUI_imgui.h + ../Common/imguiParameters.cpp + ../Common/imguiParameters.h + ../Common/DemoBase.cpp + ../Common/DemoBase.h + + ${VIS_FILES} + ${PROJECT_PATH}/Common/Common.h + + CMakeLists.txt +) + +set_target_properties(PlectonemesDemo PROPERTIES FOLDER "Demos") +set_target_properties(PlectonemesDemo PROPERTIES DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) +set_target_properties(PlectonemesDemo PROPERTIES RELWITHDEBINFO_POSTFIX ${CMAKE_RELWITHDEBINFO_POSTFIX}) +set_target_properties(PlectonemesDemo PROPERTIES MINSIZEREL_POSTFIX ${CMAKE_MINSIZEREL_POSTFIX}) +add_dependencies(PlectonemesDemo ${SIMULATION_DEPENDENCIES}) +target_link_libraries(PlectonemesDemo ${SIMULATION_LINK_LIBRARIES}) + +#### + + find_package( Eigen3 REQUIRED ) include_directories( ${EIGEN3_INCLUDE_DIR} ) include_directories(${PROJECT_PATH}/extern/freeglut/include) diff --git a/Demos/StiffRodsDemos/PlectonemesDemo.cpp b/Demos/StiffRodsDemos/PlectonemesDemo.cpp new file mode 100644 index 00000000..19eaecd7 --- /dev/null +++ b/Demos/StiffRodsDemos/PlectonemesDemo.cpp @@ -0,0 +1,294 @@ +#include "Common/Common.h" +#include "Demos/Visualization/MiniGL.h" +#include "Demos/Visualization/Selection.h" +#include "Simulation/TimeManager.h" +#include +#include "Simulation/SimulationModel.h" +#include "Simulation/TimeStepController.h" +#include +#include "Demos/Visualization/Visualization.h" +#include "Utils/Logger.h" +#include "Utils/Timing.h" +#include "Utils/FileSystem.h" +#include "Demos/Common/DemoBase.h" +#include "Simulation/Simulation.h" +#include "Simulation/DistanceFieldCollisionDetection.h" + +/* +* Trying to reproduce the results of plectonemes formation demonstrated in +* https://youtu.be/EFH9xt4omls?si=JgBwHavp-9jNoqIN&t=10 +*/ + +#define _USE_MATH_DEFINES +#include "math.h" + + +// Enable memory leak detection +#if defined(_DEBUG) && !defined(EIGEN_ALIGN) + #define new DEBUG_NEW +#endif + +using namespace PBD; +using namespace Eigen; +using namespace std; +using namespace Utilities; + +void timeStep (); +void buildModel (); +void createRodModel(); +void render (); +void reset(); +void changeSolver(); + +DemoBase *base; +DistanceFieldCollisionDetection* cd; + +const int numberOfBodies = 40; +const Real width = static_cast(0.5); +const Real height = static_cast(0.3); +const Real depth = static_cast(0.3); +const Real youngsModulus = static_cast(0.2e9); +const Real torsionModulus = static_cast(0.1e9); +const Real density = 780.; + +Vector3r initialPos; +Quaternionr initialRot; +bool directSolver = true; + + +// main +int main( int argc, char **argv ) +{ + REPORT_MEMORY_LEAKS + + base = new DemoBase(); + base->init(argc, argv, "Plectonemes (Stiff Rod) Demo"); + + SimulationModel *model = new SimulationModel(); + model->init(); + Simulation::getCurrent()->setModel(model); + + cd = new DistanceFieldCollisionDetection(); + cd->init(); + + buildModel(); + + base->createParameterGUI(); + + // OpenGL + MiniGL::setClientIdleFunc (timeStep); + MiniGL::addKeyFunc('r', reset); + MiniGL::addKeyFunc('s', changeSolver); + MiniGL::setClientSceneFunc(render); + MiniGL::setViewport (40.0, 0.1f, 500.0, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0)); + + MiniGL::mainLoop(); + + Utilities::Timing::printAverageTimes(); + Utilities::Timing::printTimeSums(); + + delete Simulation::getCurrent(); + delete base; + delete model; + delete cd; + + return 0; +} + +void reset() +{ + Utilities::Timing::printAverageTimes(); + Utilities::Timing::reset(); + + Simulation::getCurrent()->reset(); + base->getSelectedParticles().clear(); + + Simulation::getCurrent()->getModel()->cleanup(); + Simulation::getCurrent()->getTimeStep()->getCollisionDetection()->cleanup(); + + buildModel(); + +} + +void changeSolver() +{ + directSolver = !directSolver; + LOG_INFO << "using direct solver: " << directSolver; + reset(); +} + +void timeStep () +{ + const Real pauseAt = base->getValue(DemoBase::PAUSE_AT); + if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime())) + base->setValue(DemoBase::PAUSE, true); + + if (base->getValue(DemoBase::PAUSE)) + return; + + // Simulation code + SimulationModel *model = Simulation::getCurrent()->getModel(); + + auto rodLeftEdge = model->getRigidBodies()[model->getRigidBodies().size()-1]; + TimeManager* tm = TimeManager::getCurrent(); + + const unsigned int numSteps = base->getValue(DemoBase::NUM_STEPS_PER_RENDER); + const float totalRodLength = width * numberOfBodies; + for (unsigned int i = 0; i < numSteps; i++) + { + auto curOffset = tm->getTime(); + if (curOffset < totalRodLength*0.6) + rodLeftEdge->setPosition(initialPos + Vector3r(curOffset,0,0)); + + const Real maxRotationAngle = M_PI * 5; + auto angle = tm->getTime(); + if (angle < maxRotationAngle) + { + AngleAxisr angleAxis(angle, Vector3r(0, 1, 0)); + rodLeftEdge->setRotation(initialRot * Quaternionr(angleAxis)); + } + + rodLeftEdge->setRotationMatrix(rodLeftEdge->getRotation().matrix()); + + START_TIMING("SimStep"); + Simulation::getCurrent()->getTimeStep()->step(*model); + STOP_TIMING_AVG; + + base->step(); + } + + rodLeftEdge->getGeometry().updateMeshTransformation(rodLeftEdge->getPosition(), rodLeftEdge->getRotationMatrix()); +} + +void buildModel () +{ + TimeManager::getCurrent ()->setTimeStepSize (static_cast(0.0002)); + Simulation::getCurrent()->getTimeStep()->setValue(TimeStepController::NUM_SUB_STEPS, 1); + cd->setValue(CollisionDetection::CONTACT_TOLERANCE, 0.1); + SimulationModel* model = Simulation::getCurrent()->getModel(); + Simulation::getCurrent()->getTimeStep()->setCollisionDetection(*model, cd); + + createRodModel(); +} + +void render () +{ + base->render(); +} + +void createDirectSolverConstraints(SimulationModel* model, const unsigned int numConstraints, const int firstRbIndex) +{ + + // create zero-stretch, bending and twisting constraints + std::vector> constraintSegmentIndices; + std::vector constraintPositions; + std::vector averageRadii; + std::vector averageSegmentLengths; + std::vector youngsModuli(numConstraints, youngsModulus); + std::vector torsionModuli(numConstraints, torsionModulus); + + constraintSegmentIndices.reserve(numConstraints); + constraintPositions.reserve(numConstraints); + averageRadii.reserve(numConstraints); + averageSegmentLengths.reserve(numConstraints); + + for (unsigned int cID = 0; cID < numConstraints; ++cID) + { + constraintSegmentIndices.push_back(std::pair(cID + firstRbIndex, cID + firstRbIndex + 1)); + auto rb0 = model->getRigidBodies()[cID + firstRbIndex]; + auto rb1 = model->getRigidBodies()[cID + firstRbIndex + 1]; + + Vector3r x = (rb0->getPosition() + rb1->getPosition()) * 0.5f; + constraintPositions.push_back(x); + + // compute average length + Real avgLength = width; + averageSegmentLengths.push_back(avgLength); + + // compute average radius + averageRadii.push_back((height+depth)*0.25f); + } + + model->addDirectPositionBasedSolverForStiffRodsConstraint(constraintSegmentIndices, + constraintPositions, averageRadii, averageSegmentLengths, youngsModuli, torsionModuli); +} + +/** Create the rod model +*/ +void createRodModel() +{ + SimulationModel *model = Simulation::getCurrent()->getModel(); + SimulationModel::RigidBodyVector &rb = model->getRigidBodies(); + SimulationModel::ConstraintVector &constraints = model->getConstraints(); + + string fileName = FileSystem::normalizePath(base->getExePath() + "/resources/models/cube.obj"); + IndexedFaceMesh mesh; + VertexData vd; + DemoBase::loadMesh(fileName, vd, mesh, Vector3r::Zero(), Matrix3r::Identity(), Vector3r(height, width, depth)); + mesh.setFlatShading(true); + + VertexData vdEdges; + DemoBase::loadMesh(fileName, vdEdges, mesh, Vector3r::Zero(), Matrix3r::Identity(), Vector3r(height*4, width, depth*4)); + + rb.resize(numberOfBodies); + for (unsigned int i = 0; i < numberOfBodies; i++) + { + rb[i] = new RigidBody(); + + Real mass(static_cast(0.25 * M_PI) * width * height * depth * density); + + const Real Iy = static_cast(1. / 12.) * mass*(static_cast(3.)*(static_cast(0.25)*height*height) + width*width); + const Real Iz = static_cast(1. / 12.) * mass*(static_cast(3.)*(static_cast(0.25)*depth*depth) + width*width); + const Real Ix = static_cast(0.25) * mass*(static_cast(0.25)*(height*height + depth*depth)); + Vector3r inertia(Iy, Ix, Iz); // rod axis along y-axis + + Real angle(static_cast(M_PI_2)); + Vector3r axis(0., 0., 1.); + AngleAxisr angleAxis(angle, axis); + Quaternionr rotation(angleAxis); + + VertexData& curVd = (i == 0 || i == numberOfBodies - 1) ? vdEdges : vd; + + rb[i]->initBody(mass, + Vector3r((Real)i*width, 0.0, 0.0), + inertia, + rotation, + curVd, mesh); + + const std::vector& vertices = rb[i]->getGeometry().getVertexDataLocal().getVertices(); + const unsigned int nVert = static_cast(vertices.size()); + cd->addCollisionCylinder(i, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, vertices.data(), nVert, Vector2r(height, width)); + } + // Make the last end point static + rb[numberOfBodies-1]->setMass(0.0); + // create a kinematic body connected to the first rod element + // DirectPositionBasedSolverForStiffRods doesn't seem to work when both rod edges are not dynamic (i.e. have infinite mass) + auto kinematic = new RigidBody(); + kinematic->initBody(0, rb[0]->getPosition(), rb[0]->getRotation(), vdEdges, mesh); + rb.push_back(kinematic); + model->addTargetAngleMotorHingeJoint(0, numberOfBodies, kinematic->getPosition(), Vector3r(1, 0, 0)); + + initialPos = rb[0]->getPosition(); + initialRot = rb[0]->getRotation(); + + // disable collision between adjacent elements + for (unsigned int i = 0; i < numberOfBodies-1; ++i) + cd->addExclusionPair(i, i + 1); + + if (directSolver) + { + createDirectSolverConstraints(model, numberOfBodies-1, 0); + } + else + { + constraints.reserve(numberOfBodies); + for (unsigned int i = 0; i < numberOfBodies - 1; i++) + { + model->addStretchBendingTwistingConstraint(i, i + 1, Vector3r(static_cast(i) * width + static_cast(0.5) * width, 0.0, 0.0), + 0.25 * (height + depth), width, youngsModulus, torsionModulus); + } + } + + Simulation::getCurrent()->getTimeStep()->setValue(TimeStepController::MAX_ITERATIONS, 1); +} + From ba905bcc19a7cbe0088e06c58d48d0ba07cc183d Mon Sep 17 00:00:00 2001 From: Hadar Segal Date: Wed, 19 Mar 2025 16:08:07 +0200 Subject: [PATCH 3/3] CONTACT_STIFFNESS_RB was the missing link --- Demos/StiffRodsDemos/PlectonemesDemo.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/Demos/StiffRodsDemos/PlectonemesDemo.cpp b/Demos/StiffRodsDemos/PlectonemesDemo.cpp index 19eaecd7..5cc3a28c 100644 --- a/Demos/StiffRodsDemos/PlectonemesDemo.cpp +++ b/Demos/StiffRodsDemos/PlectonemesDemo.cpp @@ -43,7 +43,7 @@ void changeSolver(); DemoBase *base; DistanceFieldCollisionDetection* cd; -const int numberOfBodies = 40; +const int numberOfBodies = 60; const Real width = static_cast(0.5); const Real height = static_cast(0.3); const Real depth = static_cast(0.3); @@ -136,12 +136,12 @@ void timeStep () const float totalRodLength = width * numberOfBodies; for (unsigned int i = 0; i < numSteps; i++) { - auto curOffset = tm->getTime(); + auto curOffset = tm->getTime()*2; if (curOffset < totalRodLength*0.6) rodLeftEdge->setPosition(initialPos + Vector3r(curOffset,0,0)); - const Real maxRotationAngle = M_PI * 5; - auto angle = tm->getTime(); + const Real maxRotationAngle = M_PI * 10; + auto angle = tm->getTime()*2; if (angle < maxRotationAngle) { AngleAxisr angleAxis(angle, Vector3r(0, 1, 0)); @@ -162,11 +162,13 @@ void timeStep () void buildModel () { - TimeManager::getCurrent ()->setTimeStepSize (static_cast(0.0002)); + TimeManager::getCurrent ()->setTimeStepSize (static_cast(0.0004)); Simulation::getCurrent()->getTimeStep()->setValue(TimeStepController::NUM_SUB_STEPS, 1); - cd->setValue(CollisionDetection::CONTACT_TOLERANCE, 0.1); + cd->setValue(CollisionDetection::CONTACT_TOLERANCE, 0.1); + SimulationModel* model = Simulation::getCurrent()->getModel(); Simulation::getCurrent()->getTimeStep()->setCollisionDetection(*model, cd); + model->setValue(SimulationModel::CONTACT_STIFFNESS_RB, 100); createRodModel(); }