diff --git a/README.md b/README.md index 8c34f545..e84b7bed 100644 --- a/README.md +++ b/README.md @@ -9,38 +9,43 @@ This SOFA plugin (https://github.com/sofa-framework/sofa) provides a customized collision pipeline, designed specifically for needle insertion simulations. -When used together with SOFA haptic device plugins, the system offers tactile feedback +Coupled with SOFA haptic device plugins, the system offers tactile feedback for puncture resistance, release and friction during insertion and retraction. -This plugin has also been integrated in Unity via the [`SOFAUnity`](https://github.com/InfinyTech3D/SofaUnity) +Unity Engine integration is offered via the [`SOFAUnity`](https://github.com/InfinyTech3D/SofaUnity) plugin by InfinyTech3D for an enhanced simulation experience. Contact us for more information! ## Features - Proximity detection between the needle and tissue mesh primitives -- Needle simulation phases: puncture, insertion, retraction -- Constraint-based needle simulation during the 3 phases +- Needle insertion is simulated in phases: puncture, insertion, retraction +- Needle-tissue coupling is done using Lagrangian constraints - Support for haptic feedback such as resistance during puncture and friction during insertion - Compatible with SOFA-Unity integration for real-time interactive applications ## Installation and Setup -First review the official SOFA documentation for building and registering SOFA plugins +This plugin can be installed by following the official SOFA documentation for building and registering SOFA plugins https://sofa-framework.github.io/doc/plugins/build-a-plugin-from-sources/ + ### Build Steps -- Set up your `external_directories` directory (described in the SOFA documentation link above) -- Clone this repository into your `external_directories` directory: - - git clone https://github.com/InfinyTech3D/CollisionAlgorithm.git -- Register the path to your local `CollisionAlgorithm` repository in the CMakeLists.txt file located inside your `external_directories` directory -```sofa_add_subdirectory(plugin CollisionAlgorithm CollisionAlgorithm)``` -- Set `SOFA_EXTERNAL_DIRECTORIES` variable (preferably using CMake GUI) to point to your `external_directories` directory +- Set up the `external_directories` directory +- Clone into `external_directories`: +``` +git clone https://github.com/InfinyTech3D/CollisionAlgorithm.git +``` +- Register plugin path in the `external_directories` CMakeLists.txt. +``` +sofa_add_subdirectory(plugin CollisionAlgorithm CollisionAlgorithm) +``` +- Set the `SOFA_EXTERNAL_DIRECTORIES` variable pointing to `external_directories` - Configure and generate the SOFA solution using CMake - Compile SOFA solution (the plugin will be compiled as well) > [!IMPORTANT] -> In order to use the plugin, make sure that you have also built the downstream +> In order to use it, this plugin must be build alongside the downstream [`ConstraintGeometry`](https://github.com/InfinyTech3D/ConstraintGeometry) plugin. Supported SOFA version: v25.06 and above @@ -48,25 +53,25 @@ Supported SOFA version: v25.06 and above ## Architecture - doc: - - Documentation and screenshots of the examples + - Code documentation - scenes: - Various simple demo scenes - src/CollisionAlgorithm: - - source code of the insertion algorithm SOFA component and supporting collision pipeline classes + - SOFA components implementing the insertion algorithm and the supporting collision pipeline - regression: - - Files for automated regression testing in alignment with SOFA's testing framework + - Reference files for automated non-regression tests based on SOFA's testing framework -## Usage +## Get Started -- To use the plugin, include the `CollisionAlgorithm` plugin in your SOFA .xml scene file. +- Include the `CollisionAlgorithm` plugin in a scene file. -``` ``` -- Add the `CollisionLoop` component in the root node of your scene. +``` ``` +- Add the `CollisionLoop` component at the root node of your scene. ``` - +**** @@ -74,11 +79,11 @@ Supported SOFA version: v25.06 and above ``` -This component substitutes the default `CollisionPipeline` and manages the needle insertion algorithm. -However, the two components can co-exist, allowing users to mix the standard collision detection/constraint resolution pipelines of SOFA. +This component manages the needle insertion algorithm. It can work simultaneously with the existing SOFA `CollisionPipeline` +and can thus be added incrementally in existing SOFA scenes where contacts and collisions are modelled. -- Create a node to represent the needle and additional nodes for the needle tip and shaft geometries -Refer to the `scenes/NeedleInsertion.xml` example scene for guidance. +- Create a node to represent the needle and additional nodes for the needle tip and shaft geometries. +This step is more detailed, so refer to the examples in `scenes/NeedleInsertion.xml` for guidance. - Add an `InsertionAlgorithm` component inside the needle node as shown below. ``` diff --git a/doc/assist_doc.md b/doc/assist_doc.md deleted file mode 100644 index 998ce452..00000000 --- a/doc/assist_doc.md +++ /dev/null @@ -1,241 +0,0 @@ -\defgroup -Sofa Sofa -\defgroup -Sofa-sofa sofa -\ingroup -Sofa -\defgroup -Sofa-sofa-collisionAlgorithm collisionAlgorithm -\ingroup -Sofa-sofa -\defgroup -Sofa-sofa-collisionAlgorithm-AABBBroadPhase AABBBroadPhase -\ingroup -Sofa-sofa-collisionAlgorithm -##Description - -**AABBBroadPhase** is a real-time collision detection component that uses the Axis-Aligned Bounding Box (AABB) approach for broad-phase collision handling. - -It organizes elements into a 3D spatial grid, efficiently indexing and retrieving them to identify potential collisions, optimizing the process of narrowing down collision checks for more precise detection in 3D simulations. - - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **nbox** (sofa::type::Vec<3u, int>):
number of bbox -+ **isStatic** (bool):
Optimization: object is not moving in the scene -+ **method** (int):
chosen method to determine the boxes containing the elements -+ **thread** (int):
Number of threads -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-CollisionLoop CollisionLoop -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -CollisionLoop - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-DistanceFilter DistanceFilter -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -DistanceFilter - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **distance** (double):
Min distance -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-EdgeGeometry EdgeGeometry -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -EdgeGeometry - -\defgroup -Sofa-sofa-collisionAlgorithm-EdgeGeometry-Vec3d EdgeGeometry -\ingroup -Sofa-sofa-collisionAlgorithm-EdgeGeometry -##Description -EdgeGeometry - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **color** (sofa::type::RGBAColor):
Color of the collision model -+ **drawScaleNormal** (double):
Color of the collision model -+ **draw** (bool):
draw -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-Find2DClosestProximityAlgorithm Find2DClosestProximityAlgorithm -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -Find2DClosestProximityAlgorithm - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **drawcollision** (bool):
draw collision -+ **projectionMatrix** (sofa::type::Mat<3u, 4u, double>):
projection matrix -+ **output** (sofa::collisionAlgorithm::DetectionOutput):
output of the collision detection -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-FindClosestProximityAlgorithm FindClosestProximityAlgorithm -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -FindClosestProximityAlgorithm - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **drawcollision** (bool):
draw collision -+ **output** (sofa::collisionAlgorithm::DetectionOutput):
output of the collision detection -+ **projective** (bool):
projection of closest prox onto from element -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-FullAABBBroadPhase FullAABBBroadPhase -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -FullAABBBroadPhase - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **nbox** (sofa::type::Vec<3u, int>):
number of bbox -+ **isStatic** (bool):
Optimization: object is not moving in the scene -+ **method** (int):
chosen method to determine the boxes containing the elements -+ **thread** (int):
Number of threads -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-PointGeometry PointGeometry -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -PointGeometry - -\defgroup -Sofa-sofa-collisionAlgorithm-PointGeometry-Vec3d PointGeometry -\ingroup -Sofa-sofa-collisionAlgorithm-PointGeometry -##Description -PointGeometry - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **color** (sofa::type::RGBAColor):
Color of the collision model -+ **drawScaleNormal** (double):
Color of the collision model -+ **draw** (bool):
draw -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-SubsetGeometry SubsetGeometry -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -SubsetGeometry - -\defgroup -Sofa-sofa-collisionAlgorithm-SubsetGeometry-Vec3d SubsetGeometry -\ingroup -Sofa-sofa-collisionAlgorithm-SubsetGeometry -##Description -SubsetGeometry - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **color** (sofa::type::RGBAColor):
Color of the collision model -+ **drawScaleNormal** (double):
Color of the collision model -+ **draw** (bool):
draw -+ **indices** (sofa::type::vector >):
Indices to keep -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-TetrahedronGeometry TetrahedronGeometry -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -TetrahedronGeometry - -\defgroup -Sofa-sofa-collisionAlgorithm-TetrahedronGeometry-Vec3d TetrahedronGeometry -\ingroup -Sofa-sofa-collisionAlgorithm-TetrahedronGeometry -##Description -TetrahedronGeometry - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **color** (sofa::type::RGBAColor):
Color of the collision model -+ **drawScaleNormal** (double):
Color of the collision model -+ **draw** (bool):
draw -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - -\defgroup -Sofa-sofa-collisionAlgorithm-TriangleGeometry TriangleGeometry -\ingroup -Sofa-sofa-collisionAlgorithm -##Description -TriangleGeometry - -\defgroup -Sofa-sofa-collisionAlgorithm-TriangleGeometry-Vec3d TriangleGeometry -\ingroup -Sofa-sofa-collisionAlgorithm-TriangleGeometry -##Description -TriangleGeometry - -##Parameters: -+ **name** (std::__cxx11::basic_string, std::allocator >):
object name -+ **printLog** (bool):
if true, emits extra messages at runtime. -+ **tags** (sofa::core::objectmodel::TagSet):
list of the subsets the object belongs to -+ **bbox** (sofa::type::BoundingBox):
this object bounding box -+ **componentState** (sofa::core::objectmodel::ComponentState):
The state of the component among (Dirty, Valid, Undefined, Loading, Invalid). -+ **listening** (bool):
if true, handle the events, otherwise ignore the events -+ **color** (sofa::type::RGBAColor):
Color of the collision model -+ **drawScaleNormal** (double):
Color of the collision model -+ **draw** (bool):
draw -##Output Signals: -+ __connection__ : Signal event called at connection -+ __disconnection__ : Signal event called at connection - diff --git a/doc/diagrams/algorithm.puml b/doc/diagrams/algorithm.puml new file mode 100644 index 00000000..e3c0764d --- /dev/null +++ b/doc/diagrams/algorithm.puml @@ -0,0 +1,152 @@ +@startuml collision-algorithm-algorithm + +skinparam backgroundColor #FAFAFA +skinparam defaultFontName Sans +skinparam defaultFontSize 11 + +skinparam class { + BackgroundColor #EEF2FB + BorderColor #5577AA + HeaderBackgroundColor #7AA3CC + FontColor #222222 + HeaderFontColor #FFFFFF + HeaderFontStyle bold + ArrowColor #2E3436 + AttributeFontSize 10 +} + +skinparam class<> { + HeaderBackgroundColor #4E9A06 +} + +skinparam class<> { + HeaderBackgroundColor #75507B +} + +skinparam class<> { + HeaderBackgroundColor #888888 +} + +skinparam note { + BackgroundColor #FFFBD5 + BorderColor #C8A000 +} + +' ───────────────────────────────────────────── +' SOFA root (external) +' ───────────────────────────────────────────── +class BaseObject <> { +} + +' ───────────────────────────────────────────── +' Pipeline stubs (defined in pipeline.puml) +' ───────────────────────────────────────────── +abstract class CollisionAlgorithm <> { + + {abstract} doDetection() +} + +BaseObject <|-- CollisionAlgorithm + +' ───────────────────────────────────────────── +' Detection output (repeated for self-containedness) +' ───────────────────────────────────────────── +class "DetectionOutput" as DetectionOutput <> { + + add(first, second) + + add(other : DetectionOutput) + + size() : unsigned + + begin() : const_iterator + + end() : const_iterator + + operator[](i) : PairDetection + + back() : PairDetection + + clear() + # m_output : vector +} + +note right of DetectionOutput + PairDetection = pair + FIRST, SECOND default to BaseProximity. +end note + +' ───────────────────────────────────────────── +' BaseAlgorithm + filter (stub with filter detail) +' ───────────────────────────────────────────── +abstract class BaseAlgorithm <> { + + acceptFilter(p1, p2) : bool + + addFilter(BaseFilter::SPtr) + + getFilterFunc() : FilterFUNC + + {static} getDefaultFilterFunc() : FilterFUNC + # m_filters : vector +} + +abstract class "BaseAlgorithm::BaseFilter" as BaseFilter <> { + + l_algo : SingleLink + + init() + + {abstract} accept(p1, p2) : bool +} + +CollisionAlgorithm <|-- BaseAlgorithm +BaseAlgorithm "1" o-- "*" BaseFilter : filters > + +' ───────────────────────────────────────────── +' InsertionAlgorithm +' ───────────────────────────────────────────── +class InsertionAlgorithm { + .. geometry links .. + + l_tipGeom : SingleLink + + l_surfGeom : SingleLink + + l_shaftGeom : SingleLink + + l_volGeom : SingleLink + .. detection outputs .. + + d_collisionOutput : Data + + d_insertionOutput : Data + .. flags .. + + d_enablePuncture : Data + + d_enableInsertion : Data + + d_enableShaftCollision : Data + + d_projective : Data + .. thresholds .. + + d_punctureForceThreshold : Data + + d_tipDistThreshold : Data + .. state .. + + m_couplingPts : vector + .. methods .. + + doDetection() + + {virtual} puncturePhase() : DetectionOutput + + {virtual} shaftCollisionPhase() : DetectionOutput + + {virtual} insertionPhase() + + {virtual} reprojectCouplingPoints() : DetectionOutput +} + +note right of InsertionAlgorithm + puncturePhase, shaftCollisionPhase, and + insertionPhase are virtual — subclasses can + override individual phases without reimplementing + the full detection dispatch in doDetection(). +end note + +BaseAlgorithm <|-- InsertionAlgorithm +InsertionAlgorithm ..> DetectionOutput : produces > + +' ───────────────────────────────────────────── +' Find2DClosestProximityAlgorithm +' ───────────────────────────────────────────── +class Find2DClosestProximityAlgorithm { + + l_from : SingleLink + + l_dest : SingleLink + + d_projectionMatrix : Data + + d_output : Data + + doDetection() + + project(p : Vec3) : Vec2 + + findClosestProximity2D(prox, it) : BaseProximity::SPtr +} + +note right of Find2DClosestProximityAlgorithm + Projects 3D positions into 2D via a camera + projection matrix, then finds the closest + element by 2D screen-space distance. +end note + +BaseAlgorithm <|-- Find2DClosestProximityAlgorithm +Find2DClosestProximityAlgorithm ..> DetectionOutput : produces > + +@enduml diff --git a/doc/diagrams/broadphase.puml b/doc/diagrams/broadphase.puml new file mode 100644 index 00000000..a89c36eb --- /dev/null +++ b/doc/diagrams/broadphase.puml @@ -0,0 +1,149 @@ +@startuml collision-algorithm-broadphase + +skinparam backgroundColor #FAFAFA +skinparam defaultFontName Sans +skinparam defaultFontSize 11 + +skinparam class { + BackgroundColor #EEF2FB + BorderColor #5577AA + HeaderBackgroundColor #7AA3CC + FontColor #222222 + HeaderFontColor #FFFFFF + HeaderFontStyle bold + ArrowColor #2E3436 + AttributeFontSize 10 +} + +skinparam class<> { + HeaderBackgroundColor #4E9A06 +} + +skinparam class<> { + HeaderBackgroundColor #888888 +} + +skinparam note { + BackgroundColor #FFFBD5 + BorderColor #C8A000 +} + +' ───────────────────────────────────────────── +' SOFA root (external) +' ───────────────────────────────────────────── +class BaseObject <> { +} + +' ───────────────────────────────────────────── +' Stubs from other diagrams +' ───────────────────────────────────────────── +class BaseGeometry +class BaseElement + +' ───────────────────────────────────────────── +' BroadPhase interface (nested in BaseGeometry) +' ───────────────────────────────────────────── +abstract class "BaseGeometry::BroadPhase" as BroadPhase <> { + + l_geometry : SingleLink + + init() + + {abstract} getNbox() : Vec3i + + {abstract} getBoxCoord(P : Vec3) : Vec3i + + {abstract} getElementSet(i, j, k) : set + + getTypeInfo() : type_info& + + {abstract} initBroadPhase() + + {abstract} updateBroadPhase() +} + +note right of BroadPhase + Nested class inside BaseGeometry. + Registered as a slave object — + BaseGeometry::setBroadPhase() + calls initBroadPhase() on attachment. +end note + +BaseObject <|-- BroadPhase +BroadPhase ..> BaseGeometry : l_geometry > +BroadPhase ..> BaseElement : getElementSet() returns set of > + +' ───────────────────────────────────────────── +' AABB base +' ───────────────────────────────────────────── +abstract class BaseAABBBroadPhase <> { + + d_nbox : Data ' default (8,8,8) + + d_static : Data ' skip update if true + + d_method : Data ' 0=project, 1=SAT, 2=bbox + + d_thread : Data ' thread count for method 2 + + getBBox() : BoundingBox + + getMin() : Vec3 + + getMax() : Vec3 + + getCellSize() : Vec3 + + getBoxCoord(P) : Vec3i + + getNbox() : Vec3i + + initBroadPhase() + + updateBroadPhase() + + doUpdate() + + updateElemInBoxes() + + projectElemOnBoxes() ' method 0 + + boxTriangleSAT() ' method 1 + + bboxIntersection() ' method 2 — multithreaded + + {abstract} newContainer() + + {abstract} addElement(i, j, k, elmt) + + {abstract} updateData() + # m_Bmin : Vec3 + # m_Bmax : Vec3 + # m_cellSize : Vec3 + # m_nbox : Vec3i + # m_data : vector> +} + +note right of BaseAABBBroadPhase + Three strategies to map elements to cells, + selected at runtime via d_method: + 0 — project element centre, check residual + 1 — SAT-based triangle-box overlap test + 2 — pure bounding-box intersection (threaded) +end note + +BroadPhase <|-- BaseAABBBroadPhase + +' ───────────────────────────────────────────── +' Concrete implementations +' ───────────────────────────────────────────── +class AABBBroadPhase { + + updateData() + + getElementSet(i, j, k) : set + + newContainer() + + addElement(i, j, k, elmt) + + getKey(i, j, k) : Index + + getIKey(key) : unsigned + + getJKey(key) : unsigned + + getKKey(key) : unsigned + # m_indexedElement : map> + # m_offset : Vec<2, size_t> +} + +class FullAABBBroadPhase { + + updateData() ' no-op — no offset needed + + getElementSet(i, j, k) : set + + newContainer() + + addElement(i, j, k, elmt) + # m_indexedElement : vector>>> + # m_offset : Vec<2, size_t> +} + +note bottom of AABBBroadPhase + Sparse storage: hash map keyed by + a linearised (i,j,k) index. + Good for scenes where most cells are empty. +end note + +note bottom of FullAABBBroadPhase + Dense storage: pre-allocated 3D vector. + O(1) lookup at the cost of memory for + all nbox[0]×nbox[1]×nbox[2] cells. +end note + +BaseAABBBroadPhase <|-- AABBBroadPhase +BaseAABBBroadPhase <|-- FullAABBBroadPhase + +@enduml diff --git a/doc/diagrams/geometry-elements.puml b/doc/diagrams/geometry-elements.puml new file mode 100644 index 00000000..96cd6fb7 --- /dev/null +++ b/doc/diagrams/geometry-elements.puml @@ -0,0 +1,261 @@ +@startuml collision-algorithm-geometry-elements + +skinparam backgroundColor #FAFAFA +skinparam defaultFontName Sans +skinparam defaultFontSize 11 + +skinparam class { + BackgroundColor #EEF2FB + BorderColor #5577AA + HeaderBackgroundColor #7AA3CC + FontColor #222222 + HeaderFontColor #FFFFFF + HeaderFontStyle bold + ArrowColor #2E3436 + AttributeFontSize 10 +} + +skinparam class<> { + HeaderBackgroundColor #4E9A06 +} + +skinparam class<> { + HeaderBackgroundColor #75507B +} + +skinparam note { + BackgroundColor #FFFBD5 + BorderColor #C8A000 +} + +' ───────────────────────────────────────────── +' Stubs from other diagrams +' ───────────────────────────────────────────── +abstract class CollisionComponent <> { + + {abstract} prepareDetection() +} + +class BaseProximity + +' ───────────────────────────────────────────── +' Element container (shared by all element types) +' ───────────────────────────────────────────── +class "ElementContainer" as ElementContainer <> { + + insert(e : SPtr) + + operator[](i) : SPtr + + clear() + + size() : unsigned + + cbegin() : const_iterator + + cend() : const_iterator + + {static} empty() : const ElementContainer& + - m_data : vector +} + +' ───────────────────────────────────────────── +' Element iterator hierarchy +' ───────────────────────────────────────────── +abstract class ElementIterator <> { + + {abstract} end() : bool + + {abstract} next() + + {abstract} element() : BaseElement::SPtr + + {abstract} getTypeInfo() : type_info& + + {static} empty() : SPtr +} + +class EmptyIterator { + + end() : bool ' always true + + next() ' no-op + + element() : BaseElement::SPtr + + {static} get() : SPtr +} + +class "TDefaultElementIterator" as TDefaultElementIterator { + + end() : bool + + next() + + element() : BaseElement::SPtr + + getTypeInfo() : type_info& + # m_it : CONTAINER::const_iterator + # m_end : CONTAINER::const_iterator +} + +note right of TDefaultElementIterator + Two concrete variants: + TDefaultElementIterator_ref — wraps a reference + TDefaultElementIterator_copy — owns a copy of the container +end note + +ElementIterator <|-- EmptyIterator +ElementIterator <|-- TDefaultElementIterator + +' ───────────────────────────────────────────── +' Base element +' ───────────────────────────────────────────── +abstract class BaseElement <> { + + {abstract} pointElements() : const ElementContainer& + + {abstract} edgeElements() : const ElementContainer& + + {abstract} triangleElements() : const ElementContainer& + + {abstract} tetrahedronElements() : const ElementContainer& + + {abstract} draw(VisualParams*) + + {abstract} getTypeInfo() : type_info& + + {abstract} name() : string + + {abstract} update() + + setDirty(bool) + + isDirty() : bool + # m_isDirty : bool +} + +' ───────────────────────────────────────────── +' Concrete element types +' ───────────────────────────────────────────── +class PointElement { + + getP0() : BaseProximity::SPtr + + triangleAround() : ElementContainer& + + {static} create(prox) : SPtr + - m_prox : BaseProximity::SPtr + - m_triangleAround : ElementContainer +} + +class EdgeElement { + + getP0() : BaseProximity::SPtr + + getP1() : BaseProximity::SPtr + + {static} create(point0, point1) : SPtr + - m_pointElements : ElementContainer +} + +class TriangleElement { + + getP0() : BaseProximity::SPtr + + getP1() : BaseProximity::SPtr + + getP2() : BaseProximity::SPtr + + getTriangleInfo() : const TriangleInfo& + + {static} create(...) : SPtr + .. nested .. + TriangleInfo : v0,v1,d00,d01,d11,invDenom,area,ax1,ax2,P0,P1,P2,N + .. + - m_tinfo : TriangleInfo + - m_pointElements : ElementContainer + - m_edgeElements : ElementContainer +} + +class TetrahedronElement { + + getP0() : BaseProximity::SPtr + + getP1() : BaseProximity::SPtr + + getP2() : BaseProximity::SPtr + + getP3() : BaseProximity::SPtr + + getTetrahedronInfo() : const TetraInfo& + + {static} create(...) : SPtr + .. nested .. + TetraInfo : V0,P0-P3,ax1-ax3,ax2Cax3 + .. + - m_tinfo : TetraInfo + - m_pointElements : ElementContainer + - m_edgeElements : ElementContainer + - m_triangleElements : ElementContainer +} + +BaseElement <|-- PointElement +BaseElement <|-- EdgeElement +BaseElement <|-- TriangleElement +BaseElement <|-- TetrahedronElement + +PointElement ..> BaseProximity : holds m_prox > +PointElement *-- ElementContainer : triangleAround > +EdgeElement *-- ElementContainer : m_pointElements > +TriangleElement *-- ElementContainer : m_pointElements\nm_edgeElements > +TetrahedronElement *-- ElementContainer : m_pointElements\nm_edgeElements\nm_triangleElements > + +' ───────────────────────────────────────────── +' Geometry hierarchy +' ───────────────────────────────────────────── +abstract class BaseGeometry <> { + + d_color : Data + + d_drawScaleNormal : Data + + d_draw : Data + + {abstract} begin(id) : ElementIterator::SPtr + + end() : const BaseGeometry* + + {abstract} getSize() : unsigned + + {abstract} getPosition(pid, VecCoordId) : Vec3 + + {abstract} getVelocity(pid, VecDerivId) : Vec3 + + prepareDetection() + + buildPointElements() + + buildEdgeElements() + + buildTriangleElements() + + buildTetrahedronElements() + + setBroadPhase(BroadPhase*) + + getBroadPhase() : BroadPhase* + + pointBegin(id) : ElementIterator::SPtr + + edgeBegin(id) : ElementIterator::SPtr + + triangleBegin(id) : ElementIterator::SPtr + + tetrahedronBegin(id) : ElementIterator::SPtr + + pointElements() : ElementContainer& + + edgeElements() : ElementContainer& + + triangleElements() : ElementContainer& + + tetrahedronElements() : ElementContainer& + + internalData() : InternalDataContainer& + - m_pointElements : ElementContainer + - m_edgeElements : ElementContainer + - m_triangleElements : ElementContainer + - m_tetrahedronElements : ElementContainer + - m_broadPhase : BroadPhase* + - m_internalData : InternalDataContainer +} + +abstract class "TBaseGeometry" as TBaseGeometry <> { + + l_state : SingleLink> + + getState() : MechanicalState* + + getSize() : unsigned + + getPosition(pid, VecCoordId) : Vec3 + + getVelocity(pid, VecDerivId) : Vec3 + + storeLambda(cParams, resId, cid_global, cid_local, lambda) +} + +class "PointGeometry" as PointGeometry { + + buildPointElements() + + begin(id) : ElementIterator::SPtr +} + +class "EdgeGeometry" as EdgeGeometry { + + l_topology : SingleLink + + buildEdgeElements() + + begin(id) : ElementIterator::SPtr +} + +class "TriangleGeometry" as TriangleGeometry { + + buildTriangleElements() + + begin(id) : ElementIterator::SPtr +} + +class "TetrahedronGeometry" as TetrahedronGeometry { + + buildTetrahedronElements() + + begin(id) : ElementIterator::SPtr +} + +class "SubsetGeometry" as SubsetGeometry { + + l_geometry : SingleLink + + d_indices : Data> + + begin(id) : ElementIterator::SPtr + + prepareDetection() ' no-op + - m_elements : vector +} + +note bottom of TriangleGeometry + Geometry hierarchy is cumulative — + each level adds its element type + to those of its parent. + TetrahedronGeometry also builds + points, edges, and triangles. +end note + +CollisionComponent <|-- BaseGeometry +BaseGeometry <|-- TBaseGeometry +TBaseGeometry <|-- PointGeometry +PointGeometry <|-- EdgeGeometry +EdgeGeometry <|-- TriangleGeometry +TriangleGeometry <|-- TetrahedronGeometry +TBaseGeometry <|-- SubsetGeometry + +BaseGeometry *-- ElementContainer : 4 typed containers > +BaseGeometry ..> ElementIterator : begin() returns > +SubsetGeometry ..> BaseGeometry : l_geometry (draws elements from) > +ElementContainer o-- BaseElement : holds SPtr > + +@enduml diff --git a/doc/diagrams/operations.puml b/doc/diagrams/operations.puml new file mode 100644 index 00000000..72aebc28 --- /dev/null +++ b/doc/diagrams/operations.puml @@ -0,0 +1,188 @@ +@startuml collision-algorithm-operations + +skinparam backgroundColor #FAFAFA +skinparam defaultFontName Sans +skinparam defaultFontSize 11 + +skinparam class { + BackgroundColor #EEF2FB + BorderColor #5577AA + HeaderBackgroundColor #7AA3CC + FontColor #222222 + HeaderFontColor #FFFFFF + HeaderFontStyle bold + ArrowColor #2E3436 + AttributeFontSize 10 +} + +skinparam class<> { + HeaderBackgroundColor #4E9A06 +} + +skinparam class<> { + HeaderBackgroundColor #8F5902 +} + +skinparam note { + BackgroundColor #FFFBD5 + BorderColor #C8A000 +} + +' ───────────────────────────────────────────── +' Generic operation base templates +' ───────────────────────────────────────────── +abstract class "GenericOperation" as GenericOperation <> { + + {static} get(obj) : FUNC ' dispatches via obj->getTypeInfo() + + {static} get(type_info) : FUNC + + {static} register_func(f) : int + + {abstract} defaultFunc(PARAMS...) : RETURN + + notFound(type_info) ' virtual, empty default + - m_map : map +} + +note right of GenericOperation + Singleton per CHILD type. + Concrete element types call + register_func() at startup. + Unknown types fall back to defaultFunc(). +end note + +abstract class "GenericOperation2" as GenericOperation2 <> { + + {static} get(obj1, obj2) : FUNC + + {static} get(type_info1, type_info2) : FUNC + + {static} register_func(f) : int + + {abstract} defaultFunc(PARAMS...) : RETURN + + {abstract} notFound(type_info1, type_info2) + - m_map : map, void*> +} + +note right of GenericOperation2 + Same as GenericOperation but dispatches + on a pair of types — for operations that + depend on two element types simultaneously. +end note + +' ───────────────────────────────────────────── +' Concrete operations +' ───────────────────────────────────────────── +class "Project::Operation" as ProjectOp { + .. Result .. + distance : double + prox : BaseProximity::SPtr + .. + + defaultFunc(P, elem) : Result ' returns {max, null} + + notFound(type_info) +} + +note bottom of ProjectOp + Params: (const Vec3&, const BaseElement::SPtr&) + Returns best projection point + distance. +end note + +class "FindClosestProximity::Operation" as FindClosestOp { + + {static} broadPhaseIterator(prox, broadphase) : ElementIterator::SPtr + + defaultFunc(prox, geometry, projectOp, filter) : BaseProximity::SPtr + - doFindClosesPoint(prox, it, projectOp, filter) : BaseProximity::SPtr +} + +note bottom of FindClosestOp + Params: (BaseProximity::SPtr, BaseGeometry*, + Project::FUNC, FilterFUNC) + Uses broad phase if available, else iterates all elements. +end note + +class "ContainsPointInElement::Operation" as ContainsElemOp { + + defaultFunc(P, elem) : bool ' returns false + + notFound(type_info) +} + +note bottom of ContainsElemOp + Params: (const Vec3&, const BaseElement::SPtr&) +end note + +class "ContainsPointInProximity::Operation" as ContainsProxOp { + + defaultFunc(P, prox) : bool ' returns false + + notFound(type_info) +} + +note bottom of ContainsProxOp + Params: (const Vec3&, const BaseProximity::SPtr&) + Delegates to ContainsPointInElement via prox->element(). +end note + +class "CreateCenterProximity::Operation" as CreateCenterOp { + + defaultFunc(elem) : BaseProximity::SPtr ' returns null + + notFound(type_info) +} + +note bottom of CreateCenterOp + Params: (const BaseElement::SPtr&) +end note + +class "Needle::PrunePointsAheadOfTip" as PruneOp { + + defaultFunc(couplingPts, elem) : bool ' returns false + + notFound(type_info) +} + +note bottom of PruneOp + Params: (vector&, const BaseElement::SPtr&) + Free function: prunePointsUsingEdges(couplingPts, edgeElem) +end note + +GenericOperation <|-- ProjectOp +GenericOperation <|-- FindClosestOp +GenericOperation <|-- ContainsElemOp +GenericOperation <|-- ContainsProxOp +GenericOperation <|-- CreateCenterOp +GenericOperation <|-- PruneOp + +FindClosestOp ..> ProjectOp : calls project func > +ContainsProxOp ..> ContainsElemOp : delegates via prox->element() > + +' ───────────────────────────────────────────── +' Toolboxes — register and implement per-type functions +' ───────────────────────────────────────────── +class PointToolBox <> { + + {static} project(P, point) : Project::Result + + {static} createCenterProximity(point) : BaseProximity::SPtr +} + +class EdgeToolBox <> { + + {static} project(P, edge) : Project::Result + + {static} createCenterProximity(edge) : BaseProximity::SPtr + + {static} projectOnEdge(projP, e1, e2, fact_u, fact_v) + + {static} normalize(P0, P1, f0, f1) +} + +class TriangleToolBox <> { + + {static} project(P, tri) : Project::Result + + {static} createCenterProximity(tri) : BaseProximity::SPtr + + {static} containsPoint(P, tri) : bool + + {static} computeTriangleBaryCoords(projP, tinfo, f0, f1, f2) + + {static} projectOnTriangle(projP, tinfo, f0, f1, f2) + + {static} isInTriangle(P, tinfo, f0, f1, f2) : bool + + {static} normalize(P0, P1, P2, f0, f1, f2) +} + +class TetrahedronToolBox <> { + + {static} project(P, tetra) : Project::Result + + {static} createCenterProximity(tetra) : BaseProximity::SPtr + + {static} containsPoint(P, tetra) : bool + + {static} projectOnTetra(projP, tinfo, f0, f1, f2, f3) + + {static} computeTetraBaryCoords(P, tinfo, f0, f1, f2, f3) + + {static} isInTetra(P, tinfo, f0, f1, f2, f3) : bool + + {static} normalize(P0, P1, P2, P3, f0, f1, f2, f3) +} + +PointToolBox ..> ProjectOp : <> +PointToolBox ..> CreateCenterOp : <> +EdgeToolBox ..> ProjectOp : <> +EdgeToolBox ..> CreateCenterOp : <> +TriangleToolBox ..> ProjectOp : <> +TriangleToolBox ..> CreateCenterOp : <> +TriangleToolBox ..> ContainsElemOp : <> +TetrahedronToolBox ..> ProjectOp : <> +TetrahedronToolBox ..> CreateCenterOp : <> +TetrahedronToolBox ..> ContainsElemOp : <> + +@enduml diff --git a/doc/diagrams/overview.puml b/doc/diagrams/overview.puml new file mode 100644 index 00000000..a6c86275 --- /dev/null +++ b/doc/diagrams/overview.puml @@ -0,0 +1,208 @@ +@startuml collision-algorithm-overview + +skinparam backgroundColor #FAFAFA +skinparam defaultFontName Sans +skinparam defaultFontSize 11 + +skinparam class { + BackgroundColor #EEF2FB + BorderColor #5577AA + HeaderBackgroundColor #7AA3CC + FontColor #222222 + HeaderFontColor #FFFFFF + HeaderFontStyle bold + ArrowColor #2E3436 + AttributeFontSize 10 +} + +skinparam class<> { + HeaderBackgroundColor #4E9A06 +} + +skinparam class<> { + HeaderBackgroundColor #75507B +} + +skinparam class<> { + HeaderBackgroundColor #8F5902 +} + +skinparam class<> { + HeaderBackgroundColor #888888 +} + +skinparam package { + BackgroundColor #F5F5F5 + BorderColor #5577AA + FontColor #222222 + FontStyle bold +} + +skinparam note { + BackgroundColor #FFFBD5 + BorderColor #C8A000 +} + +' ───────────────────────────────────────────── +' Pipeline +' ───────────────────────────────────────────── +package "Pipeline" { + class BaseObject <> + abstract class CollisionComponent <> + abstract class CollisionAlgorithm <> + class CollisionLoop + + BaseObject <|-- CollisionComponent + BaseObject <|-- CollisionAlgorithm + BaseObject <|-- CollisionLoop + CollisionLoop ..> CollisionComponent : prepareDetection > + CollisionLoop ..> CollisionAlgorithm : doDetection > +} + +' ───────────────────────────────────────────── +' Geometry +' ───────────────────────────────────────────── +package "Geometry" { + abstract class BaseGeometry <> + abstract class "TBaseGeometry" as TBaseGeometry <> + class PointGeometry + class EdgeGeometry + class TriangleGeometry + class TetrahedronGeometry + class SubsetGeometry + + BaseGeometry <|-- TBaseGeometry + TBaseGeometry <|-- PointGeometry + PointGeometry <|-- EdgeGeometry + EdgeGeometry <|-- TriangleGeometry + TriangleGeometry <|-- TetrahedronGeometry + TBaseGeometry <|-- SubsetGeometry +} + +' ───────────────────────────────────────────── +' BroadPhase +' ───────────────────────────────────────────── +package "BroadPhase" { + abstract class "BaseGeometry::BroadPhase" as BroadPhase <> + abstract class BaseAABBBroadPhase <> + class AABBBroadPhase + class FullAABBBroadPhase + + BroadPhase <|-- BaseAABBBroadPhase + BaseAABBBroadPhase <|-- AABBBroadPhase + BaseAABBBroadPhase <|-- FullAABBBroadPhase +} + +' ───────────────────────────────────────────── +' Elements +' ───────────────────────────────────────────── +package "Elements" { + abstract class BaseElement <> + class "ElementContainer" as ElementContainer <> + class PointElement + class EdgeElement + class TriangleElement + class TetrahedronElement + + BaseElement <|-- PointElement + BaseElement <|-- EdgeElement + BaseElement <|-- TriangleElement + BaseElement <|-- TetrahedronElement + ElementContainer o-- BaseElement +} + +' ───────────────────────────────────────────── +' Proximity +' ───────────────────────────────────────────── +package "Proximity" { + abstract class BaseProximity <> + class PointProximity + class EdgeProximity + class TriangleProximity + class TetrahedronProximity + class FixedProximity + class "MechanicalProximity" as MechanicalProximity + class MultiProximity + + BaseProximity <|-- PointProximity + BaseProximity <|-- EdgeProximity + BaseProximity <|-- TriangleProximity + BaseProximity <|-- TetrahedronProximity + BaseProximity <|-- FixedProximity + BaseProximity <|-- MechanicalProximity + BaseProximity <|-- MultiProximity +} + +' ───────────────────────────────────────────── +' Operations +' ───────────────────────────────────────────── +package "Operations" { + abstract class "GenericOperation" as GenericOperation <> + class "Project::Operation" as ProjectOp + class "FindClosestProximity::Operation" as FindClosestOp + class "ContainsPointInElement::Operation" as ContainsElemOp + class "CreateCenterProximity::Operation" as CreateCenterOp + class "Needle::PrunePointsAheadOfTip" as PruneOp + class PointToolBox <> + class EdgeToolBox <> + class TriangleToolBox <> + class TetrahedronToolBox <> + + GenericOperation <|-- ProjectOp + GenericOperation <|-- FindClosestOp + GenericOperation <|-- ContainsElemOp + GenericOperation <|-- CreateCenterOp + GenericOperation <|-- PruneOp +} + +' ───────────────────────────────────────────── +' Algorithms +' ───────────────────────────────────────────── +package "Algorithms" { + abstract class BaseAlgorithm <> + abstract class "BaseAlgorithm::BaseFilter" as BaseFilter <> + class DistanceFilter + class InsertionAlgorithm + class Find2DClosestProximityAlgorithm + + BaseAlgorithm <|-- InsertionAlgorithm + BaseAlgorithm <|-- Find2DClosestProximityAlgorithm + BaseAlgorithm o-- BaseFilter + BaseFilter <|-- DistanceFilter +} + +' ───────────────────────────────────────────── +' DetectionOutput (shared data type) +' ───────────────────────────────────────────── +class "DetectionOutput" as DetectionOutput <> + +' ───────────────────────────────────────────── +' Inter-package relationships (one per relationship, not per type) +' ───────────────────────────────────────────── + +' Pipeline → Geometry / Algorithms (via inheritance) +CollisionComponent <|-- BaseGeometry +CollisionAlgorithm <|-- BaseAlgorithm + +' Geometry ↔ BroadPhase +BaseGeometry ..> BroadPhase : uses > + +' Geometry → Elements +BaseGeometry *-- ElementContainer : owns > + +' Elements → Proximity +BaseElement ..> BaseProximity : references > + +' Algorithms → Geometry +BaseAlgorithm ..> BaseGeometry : links to > + +' Algorithms → Operations +BaseAlgorithm ..> GenericOperation : dispatches via > + +' Algorithms → DetectionOutput +BaseAlgorithm ..> DetectionOutput : produces > + +' DetectionOutput → Proximity +DetectionOutput o-- BaseProximity : contains pairs of > + +@enduml diff --git a/doc/diagrams/pipeline.puml b/doc/diagrams/pipeline.puml new file mode 100644 index 00000000..05826933 --- /dev/null +++ b/doc/diagrams/pipeline.puml @@ -0,0 +1,114 @@ +@startuml collision-algorithm-pipeline + +skinparam backgroundColor #FAFAFA +skinparam defaultFontName Sans +skinparam defaultFontSize 11 + +skinparam class { + BackgroundColor #EEF2FB + BorderColor #5577AA + HeaderBackgroundColor #7AA3CC + FontColor #222222 + HeaderFontColor #FFFFFF + HeaderFontStyle bold + ArrowColor #2E3436 + AttributeFontSize 10 +} + +skinparam class<> { + HeaderBackgroundColor #4E9A06 +} + +skinparam class<> { + HeaderBackgroundColor #75507B +} + +skinparam class<> { + HeaderBackgroundColor #888888 +} + +skinparam note { + BackgroundColor #FFFBD5 + BorderColor #C8A000 +} + +' ───────────────────────────────────────────── +' SOFA root (external) +' ───────────────────────────────────────────── +class BaseObject <> { +} + +' ───────────────────────────────────────────── +' Pipeline interfaces +' ───────────────────────────────────────────── +abstract class CollisionComponent <> { + + {abstract} prepareDetection() +} + +abstract class CollisionAlgorithm <> { + + {abstract} doDetection() +} + +class CollisionLoop { + + handleEvent(Event*) + .. nested visitors .. + UpdateComponentVisitor + UpdateAlgorithmVisitor +} + +BaseObject <|-- CollisionComponent +BaseObject <|-- CollisionAlgorithm +BaseObject <|-- CollisionLoop + +CollisionLoop ..> CollisionComponent : discovers via visitor > +CollisionLoop ..> CollisionAlgorithm : discovers via visitor > + +' ───────────────────────────────────────────── +' Algorithm base + filters +' ───────────────────────────────────────────── +class BaseAlgorithm { + + acceptFilter(p1, p2) : bool + + addFilter(filter) + + getFilterFunc() : FilterFUNC + + {static} getDefaultFilterFunc() : FilterFUNC + # m_filters : vector +} + +abstract class "BaseAlgorithm::BaseFilter" as BaseFilter <> { + + l_algo : SingleLink + + init() + + {abstract} accept(p1, p2) : bool +} + +class DistanceFilter { + + d_distance : Data + + accept(p1, p2) : bool +} + +CollisionAlgorithm <|-- BaseAlgorithm +BaseAlgorithm "1" o-- "*" BaseFilter : filters > +BaseFilter <|-- DistanceFilter + +' ───────────────────────────────────────────── +' Detection output +' ───────────────────────────────────────────── +class "DetectionOutput" as DetectionOutput <> { + + add(first, second) + + add(other : DetectionOutput) + + size() : unsigned + + begin() : const_iterator + + end() : const_iterator + + operator[](i) : PairDetection + + back() : PairDetection + + clear() + # m_output : vector +} + +note right of DetectionOutput + FIRST, SECOND default to BaseProximity. + PairDetection = pair +end note + +BaseAlgorithm ..> DetectionOutput : produces > + +@enduml diff --git a/doc/diagrams/proximity.puml b/doc/diagrams/proximity.puml new file mode 100644 index 00000000..968ff5d8 --- /dev/null +++ b/doc/diagrams/proximity.puml @@ -0,0 +1,182 @@ +@startuml collision-algorithm-proximity + +skinparam backgroundColor #FAFAFA +skinparam defaultFontName Sans +skinparam defaultFontSize 11 + +skinparam class { + BackgroundColor #EEF2FB + BorderColor #5577AA + HeaderBackgroundColor #7AA3CC + FontColor #222222 + HeaderFontColor #FFFFFF + HeaderFontStyle bold + ArrowColor #2E3436 + AttributeFontSize 10 +} + +skinparam class<> { + HeaderBackgroundColor #4E9A06 +} + +skinparam note { + BackgroundColor #FFFBD5 + BorderColor #C8A000 +} + +' ───────────────────────────────────────────── +' Element stubs (defined in geometry-elements.puml) +' ───────────────────────────────────────────── +class PointElement +class EdgeElement +class TriangleElement +class TetrahedronElement +class "TBaseGeometry" as TBaseGeometry + +' ───────────────────────────────────────────── +' Base proximity classes +' ───────────────────────────────────────────── +abstract class BaseBaseProximity <> { + + {abstract} getPosition(VecCoordId) : Vec3 + + {abstract} getVelocity(VecDerivId) : Vec3 + + {abstract} buildJacobianConstraint(cId, dirs, fact, constraintId) + + {abstract} storeLambda(cParams, res, cid_global, cid_local, lambda) + + getTypeInfo() : type_info& +} + +abstract class BaseProximity <> { + + {abstract} getTypeInfo() : type_info& + + {abstract} copy() : SPtr + + {abstract} isNormalized() : bool + + {abstract} normalize() + + {abstract} getBaryCoord() : double* + + {static} create(args) : PROXIMITY::SPtr +} + +note right of BaseProximity + Inherits BaseBaseProximity virtually. + Provides empty default implementations + of all BaseBaseProximity virtuals; + subclasses override selectively. +end note + +BaseBaseProximity <|-- BaseProximity + +' ───────────────────────────────────────────── +' Concrete proximity types +' ───────────────────────────────────────────── +class PointProximity { + + element() : PointElement::SPtr + + getPosition(VecCoordId) : Vec3 + + getVelocity(VecDerivId) : Vec3 + + isNormalized() : bool ' always true + + getBaryCoord() : double* ' nullptr + # m_elmt : PointElement::SPtr +} + +class EdgeProximity { + + f0() : double + + f1() : double + + element() : EdgeElement::SPtr + + getPosition(VecCoordId) : Vec3 + + getVelocity(VecDerivId) : Vec3 + + getBaryCoord() : double* ' [f0, f1] + + isNormalized() : bool + + normalize() + # m_elmt : EdgeElement::SPtr + # m_f0 : double + # m_f1 : double +} + +class TriangleProximity { + + f0() : double + + f1() : double + + f2() : double + + element() : TriangleElement::SPtr + + getPosition(VecCoordId) : Vec3 + + getVelocity(VecDerivId) : Vec3 + + getBaryCoord() : double* ' [f0, f1, f2] + + isNormalized() : bool + + normalize() + # m_elmt : TriangleElement::SPtr + # m_f0 : double + # m_f1 : double + # m_f2 : double +} + +class TetrahedronProximity { + + f0() : double + + f1() : double + + f2() : double + + f3() : double + + element() : TetrahedronElement::SPtr + + getPosition(VecCoordId) : Vec3 + + getVelocity(VecDerivId) : Vec3 + + getBaryCoord() : double* ' [f0, f1, f2, f3] + + isNormalized() : bool + + normalize() + # m_elmt : TetrahedronElement::SPtr + # m_f0 : double + # m_f1 : double + # m_f2 : double + # m_f3 : double +} + +class FixedProximity { + + getNormal() : Vec3d + + getPosition(VecCoordId) : Vec3 + + getVelocity(VecDerivId) : Vec3 ' always (0,0,0) + + isNormalized() : bool ' always true + + getBaryCoord() : double* ' nullptr + - m_position : Vec3d + - m_normal : Vec3d +} + +class "MechanicalProximity" as MechanicalProximity { + + getPId() : unsigned + + getGeometry() : BaseGeometry* + + getTGeometry() : TBaseGeometry* + + addContributions(c_it, N, fact) + + getPosition(VecCoordId) : Vec3 + + getVelocity(VecDerivId) : Vec3 + + isNormalized() : bool ' always true + + getBaryCoord() : double* ' nullptr + # m_geometry : TBaseGeometry* + # m_pid : unsigned +} + +class MultiProximity { + + getPosition(VecCoordId) : Vec3 ' average of all + + getVelocity(VecDerivId) : Vec3 ' average of all + + isNormalized() : bool ' always true + + getBaryCoord() : double* ' nullptr + # m_proximities : vector +} + +note right of MultiProximity + Position/velocity = average over + all aggregated proximities. +end note + +note bottom of BaseProximity + Consumed by ConstraintGeometry to build + Lagrangian constraints (buildJacobianConstraint + and storeLambda are the solver interface). +end note + +BaseProximity <|-- PointProximity +BaseProximity <|-- EdgeProximity +BaseProximity <|-- TriangleProximity +BaseProximity <|-- TetrahedronProximity +BaseProximity <|-- FixedProximity +BaseProximity <|-- MechanicalProximity +BaseProximity <|-- MultiProximity + +PointProximity ..> PointElement : holds > +EdgeProximity ..> EdgeElement : holds > +TriangleProximity ..> TriangleElement : holds > +TetrahedronProximity ..> TetrahedronElement : holds > +MechanicalProximity ..> TBaseGeometry : references > +MultiProximity "1" o-- "*" BaseProximity : aggregates > + +@enduml diff --git a/doc/doc.md b/doc/doc.md new file mode 100644 index 00000000..928e0e8e --- /dev/null +++ b/doc/doc.md @@ -0,0 +1,185 @@ +# Summary: CollisionAlgorithm & ConstraintGeometry Repositories + +The documentation here includes two SOFA plugins. Although spread across two distinct repositories, these plugins work together +to provide a complete **needle insertion simulation model**, forming a two-stage pipeline: detection → constraint resolution. +For completeness, the documentation is centralized in one document. + +--- + +## CollisionAlgorithm Repository + +### Purpose +This plugin drives the needle insertion simulation logic and provides proximity pairs to be consumed by the constraint resolution layer. It does so through nearest-neighbor queries: for each needle element, find the geometrically closest tissue element and return its barycentric parameterization on the live deforming mesh. + +### Core Architecture + +**Layered Design:** + +The plugin is organized in four layers. At the top sits the algorithm layer, which orchestrates detection. +Beneath it, the geometry layer provides components that attach to a `MechanicalState` in the scene graph, +decompose its topology into typed elements (points, edges, triangles, tetrahedra), and expose them for spatial queries. +The element and proximity layers carry the geometric primitives and their barycentric parameterizations that the algorithm layer ultimately queries and stores. + +``` +Algorithm Layer (InsertionAlgorithm) + ↓ +Geometry Layer (Point/Edge/Triangle/TetrahedronGeometry) + ↓ +Element Layer (PointElement, EdgeElement, TriangleElement, TetrahedronElement) + ↓ +Proximity Layer (EdgeProximity, TriangleProximity, etc.) +``` + +**Key Class Hierarchies:** + +1. **Geometry Hierarchy** - Progressive complexity through inheritance: + - `PointGeometry` → `EdgeGeometry` → `TriangleGeometry` → `TetrahedronGeometry` + - Each adds element types from parent + - `SubsetGeometry` wraps any geometry to expose a filtered index subset + +2. **Element Classes** - Geometric primitives with caching: + - Store precomputed data (normals, areas, barycentric denominators) + - Use dirty flags for lazy recomputation + +3. **Proximity Classes** - Interpolated contact points: + - `EdgeProximity`, `TriangleProximity`, `TetrahedronProximity` + - Store barycentric coordinates and provide position/velocity interpolation + +4. **Broad-Phase System** - AABB spatial hashing: + - Partitions space into 8×8×8 grid + - Supports multithreading and SAT-based intersection tests + +5. **Generic Operation Framework** - Type-dispatched operations: + - `Project`: Find closest point on element + - `FindClosestProximity`: Spatial search with filtering + - Factory pattern for runtime type dispatch + +### Algorithms + +**InsertionAlgorithm** — the main needle insertion algorithm. Each detection step runs one of two mutually exclusive mode sequences depending on whether puncture has occurred (`m_couplingPts` empty): + +*Before puncture* (`m_couplingPts` empty): +1. **Puncture Phase**: Finds the closest surface proximity to the needle tip. When the constraint force exceeds `punctureForceThreshold`, records the contact as a coupling point, transitioning to insertion mode. +2. **Shaft Collision Phase**: Finds closest surface proximities along the needle shaft (skipped if puncture just occurred in the same step). + +*After puncture* (`m_couplingPts` non-empty): +3. **Insertion Phase**: Adds new shaft↔volume coupling points as the needle tip advances past `tipDistThreshold`. +4. **Reprojection Phase**: Reprojects stored coupling points back onto the current shaft geometry. + +**Outputs:** +- `d_collisionOutput` → proximity pairs from the puncture and shaft collision phases +- `d_insertionOutput` → shaft↔tissue coupling pairs produced by reprojection + +**Find2DClosestProximityAlgorithm** — finds closest proximities between two geometries using a 2D projection matrix to restrict the search plane. + +--- + +## ConstraintGeometry Repository + +### Purpose +Takes collision detection output from the `CollisionAlgorithm` plugin and creates **Lagrangian constraints** prepping them for resolution by the physics solver. +Handles bilateral, unilateral, and friction-based constraints. + +### Core Architecture + +**Constraint Pipeline:** +``` +Detection Output (from CollisionAlgorithm) + ↓ +TBaseConstraint (processGeometricalData) + ↓ +InternalConstraint (proximity pairs + normals) + ↓ +ConstraintResolution (solver kernels) + ↓ +Force Application (storeLambda) +``` + +**Key Class Hierarchies:** + +1. **Constraint Types:** + - `ConstraintBilateral` - Equality constraints (position coupling) + - `ConstraintUnilateral` - Inequality constraints with optional Coulomb friction + - `ConstraintInsertion` - Specialized insertion physics with friction + +2. **Constraint Direction Strategies** - How to compute constraint normals: + - `BindDirection`: Direct relative position + - `ContactDirection`: Uses surface normal handler + - `FirstDirection`/`SecondDirection`: Normal from one proximity + - `FixedFrameDirection`: Fixed orthogonal frame + +3. **Normal Handler Strategies** - Geometry-dependent normal computation: + - `GouraudTriangleNormalHandler`: Face normals + - `PhongTriangleNormalHandler`: Smooth interpolated normals + - `EdgeNormalHandler`: Edge direction + - `GravityPointNormalHandler`: Radial from center + +4. **Constraint Resolution Classes** - Solver kernels: + - `BilateralConstraintResolution1/2/3` - 1D/2D/3D bilateral + - `UnilateralConstraintResolution` - 1D contact + - `UnilateralFrictionResolution` - 3D contact + Coulomb friction + - `InsertionConstraintResolution` - Specialized insertion solver + +--- + +## How They Work Together + +``` +┌────────────────────────────────────────────────────────────────┐ +│ CollisionAlgorithm Plugin │ +│ │ +│ ┌──────────────┐ ┌─────────────┐ ┌──────────────┐ │ +│ │ Geometries │──▶│ Broad-Phase │──▶│ InsertionAlg │ │ +│ │ (Needle/ │ │ (AABB Grid) │ │ (Detection) │ │ +│ │ Tissue) │ └─────────────┘ └──────┬───────┘ │ +│ └──────────────┘ │ │ +│ ▼ │ +│ ┌───────────────────────────────┐ │ +│ │ DetectionOutput │ │ +│ │ (Pairs of proximity points) │ │ +│ └───────────────┬───────────────┘ │ +└──────────────────────────────────────────────┼─────────────────┘ + │ + ▼ +┌────────────────────────────────────────────────────────────────┐ +│ ConstraintGeometry Plugin │ +│ │ +│ ┌──────────────────┐ ┌──────────────────┐ │ +│ │ TBaseConstraint │──▶ │ ConstraintDir │ │ +│ │ (Bilateral/ │ │ + NormalHandler │ │ +│ │ Unilateral/ │ └────────┬─────────┘ │ +│ │ Insertion) │ │ │ +│ └────────┬─────────┘ ▼ │ +│ │ ┌──────────────────┐ │ +│ │ │ ConstraintNormal │ │ +│ │ │ (directions) │ │ +│ │ └────────┬─────────┘ │ +│ ▼ ▼ │ +│ ┌───────────────────────────────────────────┐ │ +│ │ InternalConstraint (pairs + normals) │ │ +│ └────────────────────┬──────────────────────┘ │ +│ ▼ │ +│ ┌───────────────────────────────────────────┐ │ +│ │ ConstraintResolution (solver kernels) │──▶ Forces │ +│ └───────────────────────────────────────────┘ │ +└────────────────────────────────────────────────────────────────┘ +``` + +### Key Integration Points + +1. **Shared Data Type**: `DetectionOutput` contains pairs of `BaseProximity` subclasses that both plugins understand + +2. **Proximity Abstraction**: Both plugins use the same proximity hierarchy (`EdgeProximity`, `TriangleProximity`, etc.) - defined in CollisionAlgorithm, consumed by ConstraintGeometry + +3. **CollisionComponent Interface**: `BaseNormalHandler` in ConstraintGeometry inherits from `CollisionComponent` (from CollisionAlgorithm) to participate in the detection preparation phase + +4. **Generic Operation System**: Both plugins use the same `GenericOperation` factory pattern for type-dispatched operations + +--- + +## Summary + +- **CollisionAlgorithm** handles the "where" - detecting collision points between needle and tissue through geometric queries and spatial indexing +- **ConstraintGeometry** handles the "how" - converting those detection points into physical constraints with proper force resolution + +Together, they enable realistic needle insertion simulations with puncture resistance, friction, and tissue deformation.