LICENSE

@@ -2,6 +2,8 @@ Software License Agreement (BSD License)
 
  Copyright (c) 2008-2014, Willow Garage, Inc.
  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ Copyright (c) 2014-2023, CNRS
+ Copyright (c) 2018-2025, INRIA
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without

NEWS

-                                                                -*- outline -*-
-New in 1.1.1
-* Fix a bug in assimp loading procedure.
-New in 1.1.0
-* Fix uninitialized closest points in capsulePlaneIntersect.
-* Refactor sphereTriangleIntersect.
-* Initialize closest points to nan value in distance query
-* Add Converter from AABB to OBBRSS.
-* Add node type as attribute of MeshLoader.
-* Simplify box - sphere distance computation.
-* Implement GJKSolver_indep::shapeIntersect for sphere - box
-* Fix MeshShapeCollisionTraversalNodeOBB.
-* Add missing include guard in fwd.hh/
-
-New in 1.0.2
-* A lot of cleaning among functions that were not used
-  - broadphase has been removed,
-  - cost and cost source have been removed,
-
-* CollisionRequest is passed to low level functions in order to unify
-    - the calling function tree among the various possible options,
-
-* Collision test triangle - triangle
-  - in GJK, a detection of loop that made the algorithm fail to detect
-    collision has been removed,
-  - collision and distance computation between triangles in
-    MeshCollisionTraversalNode::leafTesting now uses GJK
-  - a test on triangle - triangle interaction has been added optimality of
-    closest points is check using Karush-Kuhn-Tucker conditions.
-
-New in 1.0.1
-* Fix octomap version handling,
-  - add flags related to Octomap in pkg-config file.

README.md

-## FCL -- The Flexible Collision Library
+# Coal — An extension of the Flexible Collision Library
 
-[![Building Status](https://travis-ci.org/humanoid-path-planner/hpp-fcl.svg?branch=master)](https://travis-ci.org/humanoid-path-planner/hpp-fcl)
-[![Pipeline status](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/commits/master)
-[![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-fcl/master/coverage/)
+<p align="center">
+  <a href="https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/commits/master/"><img src="https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/pipeline.svg" alt="Pipeline status"/></a>
+  <a href="https://gepettoweb.laas.fr/hpp/hpp-fcl/doxygen-html/index.html"><img src="https://img.shields.io/badge/docs-online-brightgreen" alt="Documentation"/></a>
+  <a href="http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-fcl/master/coverage/"><img src="https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/coverage.svg?job=doc-coverage" alt="Coverage report"/></a>
+  <a href="https://anaconda.org/conda-forge/hpp-fcl"><img src="https://img.shields.io/conda/dn/conda-forge/hpp-fcl.svg" alt="Conda Downloads"/></a>
+  <a href="https://anaconda.org/conda-forge/hpp-fcl"><img src="https://img.shields.io/conda/vn/conda-forge/hpp-fcl.svg" alt="Conda Version"/></a>
+  <a href="https://badge.fury.io/py/hpp-fcl"><img src="https://badge.fury.io/py/hpp-fcl.svg" alt="PyPI version"></a>
+  <a href="https://github.com/psf/black"><img alt="black" src="https://img.shields.io/badge/code%20style-black-000000.svg"></a>
+  <a href="https://github.com/astral-sh/ruff"><img alt="ruff" src="https://img.shields.io/endpoint?url=https://raw.githubusercontent.com/astral-sh/ruff/main/assets/badge/v2.json"></a>
+</p>
 
-This project is a fork from https://github.com/flexible-collision-library/fcl.
+[FCL](https://github.com/flexible-collision-library/fcl) was forked in 2015, creating a new project called HPP-FCL.
+Since then, a large part of the code has been rewritten or removed (unused and untested code), and new features have been introduced (see below).
+Due to these major changes, it was decided in 2024 to rename the HPP-FCL project to **Coal**.
 
-The main differences are.
-- the use of a safety margin when detecting collision,
-- the computation of a lower bound of the distance between two objects when collision checking is performed and no collision is found.
+If you use **Coal** in your projects and research papers, we would appreciate it if you would [cite it](https://raw.githubusercontent.com/coal-library/coal/devel/CITATION.bib).
+
+## New features
+
+Compared to the original [FCL](https://github.com/flexible-collision-library/fcl) library, the main new features are:
+- dedicated and efficient implementations of the GJK and the EPA algorithms (we do not rely on [libccd](https://github.com/danfis/libccd))
+- the support of safety margins for collision detection
+- an accelerated version of collision detection *à la Nesterov*, which leads to increased performance (up to a factor of 2). More details are available in this [paper](https://hal.archives-ouvertes.fr/hal-03662157/)
+- the computation of a lower bound of the distance between two objects when collision checking is performed, and no collision is found
+- the implementation of Python bindings for easy code prototyping
+- the support of new geometries such as height fields, capsules, ellipsoids, etc.
+- enhance reliability with the fix of a myriad of bugs
+- efficient computation of **contact points** and **contact patches** between objects
+- full support of object serialization via Boost.Serialization
+
+Note: the broad phase was reintroduced by [Justin Carpentier](https://github.com/jcarpent) in 2022, based on the FCL version 0.7.0.
+
+This project is now used in several robotics frameworks such as [Pinocchio](https://github.com/stack-of-tasks/pinocchio), an open-source library which implements efficient and versatile rigid-body dynamics algorithms, the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc), an open-source library for Motion and Manipulation Planning. **Coal** has recently also been used to develop [Simple](https://github.com/Simple-Robotics/Simple), a new (differentiable) and efficient simulator for robotics and beyond.
+
+## A high-performance library
+
+Unlike the original FCL library, Coal implements the well-established [GJK algorithm](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm) and [its variants](https://hal.archives-ouvertes.fr/hal-03662157/) for collision detection and distance computation. These implementations lead to state-of-the-art performance, as shown in the figures below.
+
+On the one hand, we have benchmarked Coal against major state-of-the-art software alternatives:
+
+1. the [Bullet simulator](https://github.com/bulletphysics/bullet3),
+2. the original [FCL library](https://github.com/flexible-collision-library/fcl) (used in the [Drake framework]()),
+3. the [libccd library](https://github.com/danfis/libccd) (used in [MuJoCo](http://mujoco.org/)).
+
+The results are depicted in the following figure, which notably shows that the accelerated variants of GJK largely outperform by a large margin (from 5x up to 15x times faster).
+Please notice that the y-axis is in log scale.
+
+<p align="center">
+  <img src="./doc/images/coal-vs-the-rest-of-the-world.png" width="600" alt="Coal vs the rest of the world" align="center"/>
+</p>
+
+On the other hand, why do we care about dedicated collision detection solvers like GJK for the narrow phase? Why can't we simply formulate the collision detection problem as a quadratic problem and call an off-the-shelf optimization solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite))? Here is why:
+
+<p align="center">
+  <img src="./doc/images/coal-performances.jpg" width="600" alt="Coal vs generic QP solvers" align="center"/>
+</p>
+
+One can observe that GJK-based approaches largely outperform solutions based on classic optimization solvers (e.g., QP solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite)), notably for large geometries composed of tens or hundreds of vertices.
+
+## Open-source projects relying on Pinocchio
+
+- [Pinocchio](https://github.com/stack-of-tasks/pinocchio) A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives.
+- [IfcOpenShell](https://github.com/IfcOpenShell/IfcOpenShell) Open source IFC library and geometry engine.
+- [Crocoddyl](https://github.com/loco-3d/crocoddyl) A software to realize model predictive control for complex robotics platforms.
+- [TSID](https://github.com/stack-of-tasks/tsid/) A software that implements a Task Space Inverse Dynamics QP.
+- [HPP](https://humanoid-path-planner.github.io/hpp-doc/) A SDK that implements motion planners for humanoids and other robots.
+- [Jiminy](https://github.com/duburcqa/jiminy) A simulator based on Pinocchio.
+- [ocs2](https://github.com/leggedrobotics/ocs2) A toolbox for Optimal Control for Switched Systems (OCS2)
+
+## Installation
+
+### Conda
+
+Coal can be installed from the [conda-forge channel](https://anaconda.org/conda-forge/coal):
+
+```bash
+conda install coal -c conda-forge
+```
+
+## Build
+
+You can find build instruction [here](./development/build.md).
+
+## C++ example
+Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install coal`.
+The `.so` library, include files and python bindings will then be installed under `$CONDA_PREFIX/lib`, `$CONDA_PREFIX/include` and `$CONDA_PREFIX/lib/python3.XX/site-packages`.
+
+Here is an example of using Coal in C++:
+```cpp
+#include "coal/math/transform.h"
+#include "coal/mesh_loader/loader.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/collision.h"
+#include "coal/collision_data.h"
+#include <iostream>
+#include <memory>
+
+// Function to load a convex mesh from a `.obj`, `.stl` or `.dae` file.
+//
+// This function imports the object inside the file as a BVHModel, i.e. a point cloud
+// which is hierarchically transformed into a tree of bounding volumes.
+// The leaves of this tree are the individual points of the point cloud
+// stored in the `.obj` file.
+// This BVH can then be used for collision detection.
+//
+// For better computational efficiency, we sometimes prefer to work with
+// the convex hull of the point cloud. This insures that the underlying object
+// is convex and thus very fast collision detection algorithms such as
+// GJK or EPA can be called with this object.
+// Consequently, after creating the BVH structure from the point cloud, this function
+// also computes its convex hull.
+std::shared_ptr<coal::ConvexBase> loadConvexMesh(const std::string& file_name) {
+  coal::NODE_TYPE bv_type = coal::BV_AABB;
+  coal::MeshLoader loader(bv_type);
+  coal::BVHModelPtr_t bvh = loader.load(file_name);
+  bvh->buildConvexHull(true, "Qt");
+  return bvh->convex;
+}
+
+int main() {
+  // Create the coal shapes.
+  // Coal supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes,
+  // halfspace and convex meshes (i.e. convex hulls of clouds of points).
+  // It also supports BVHs (bounding volumes hierarchies), height-fields and octrees.
+  std::shared_ptr<coal::Ellipsoid> shape1 = std::make_shared<coal::Ellipsoid>(0.7, 1.0, 0.8);
+  std::shared_ptr<coal::ConvexBase> shape2 = loadConvexMesh("../path/to/mesh/file.obj");
+
+  // Define the shapes' placement in 3D space
+  coal::Transform3s T1;
+  T1.setQuatRotation(coal::Quaternion3f::UnitRandom());
+  T1.setTranslation(coal::Vec3s::Random());
+  coal::Transform3s T2 = coal::Transform3s::Identity();
+  T2.setQuatRotation(coal::Quaternion3f::UnitRandom());
+  T2.setTranslation(coal::Vec3s::Random());
+
+  // Define collision requests and results.
+  //
+  // The collision request allows to set parameters for the collision pair.
+  // For example, we can set a positive or negative security margin.
+  // If the distance between the shapes is less than the security margin, the shapes
+  // will be considered in collision.
+  // Setting a positive security margin can be usefull in motion planning,
+  // i.e to prevent shapes from getting too close to one another.
+  // In physics simulation, allowing a negative security margin may be usefull to stabilize the simulation.
+  coal::CollisionRequest col_req;
+  col_req.security_margin = 1e-1;
+  // A collision result stores the result of the collision test (signed distance between the shapes,
+  // witness points location, normal etc.)
+  coal::CollisionResult col_res;
+
+  // Collision call
+  coal::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res);
+
+  // We can access the collision result once it has been populated
+  std::cout << "Collision? " << col_res.isCollision() << "\n";
+  if (col_res.isCollision()) {
+    coal::Contact contact = col_res.getContact(0);
+    // The penetration depth does **not** take into account the security margin.
+    // Consequently, the penetration depth is the true signed distance which separates the shapes.
+    // To have the distance which takes into account the security margin, we can simply add the two together.
+    std::cout << "Penetration depth: " << contact.penetration_depth << "\n";
+    std::cout << "Distance between the shapes including the security margin: " << contact.penetration_depth + col_req.security_margin << "\n";
+    std::cout << "Witness point on shape1: " << contact.nearest_points[0].transpose() << "\n";
+    std::cout << "Witness point on shape2: " << contact.nearest_points[1].transpose() << "\n";
+    std::cout << "Normal: " << contact.normal.transpose() << "\n";
+  }
+
+  // Before calling another collision test, it is important to clear the previous results stored in the collision result.
+  col_res.clear();
+
+  return 0;
+}
+```
+
+## Python example
+Here is the C++ example from above translated in python using the python bindings of Coal:
+```python
+import numpy as np
+import coal
+# Optional:
+# The Pinocchio library is a rigid body algorithms library and has a handy SE3 module.
+# It can be installed as simply as `conda -c conda-forge install pinocchio`.
+# Installing pinocchio also installs coal.
+import pinocchio as pin
+
+def loadConvexMesh(file_name: str):
+    loader = coal.MeshLoader()
+    bvh: coal.BVHModelBase = loader.load(file_name)
+    bvh.buildConvexHull(True, "Qt")
+    return bvh.convex
+
+if __name__ == "__main__":
+    # Create coal shapes
+    shape1 = coal.Ellipsoid(0.7, 1.0, 0.8)
+    shape2 = loadConvexMesh("../path/to/mesh/file.obj")
+
+    # Define the shapes' placement in 3D space
+    T1 = coal.Transform3s()
+    T1.setTranslation(pin.SE3.Random().translation)
+    T1.setRotation(pin.SE3.Random().rotation)
+    T2 = coal.Transform3s();
+    # Using np arrays also works
+    T1.setTranslation(np.random.rand(3))
+    T2.setRotation(pin.SE3.Random().rotation)
+
+    # Define collision requests and results
+    col_req = coal.CollisionRequest()
+    col_res = coal.CollisionResult()
+
+    # Collision call
+    coal.collide(shape1, T1, shape2, T2, col_req, col_res)
+
+    # Accessing the collision result once it has been populated
+    print("Is collision? ", {col_res.isCollision()})
+    if col_res.isCollision():
+        contact: coal.Contact = col_res.getContact(0)
+        print("Penetration depth: ", contact.penetration_depth)
+        print("Distance between the shapes including the security margin: ", contact.penetration_depth + col_req.security_margin)
+        print("Witness point shape1: ", contact.getNearestPoint1())
+        print("Witness point shape2: ", contact.getNearestPoint2())
+        print("Normal: ", contact.normal)
+
+    # Before running another collision call, it is important to clear the old one
+    col_res.clear()
+```
+
+## Acknowledgments
+
+The development of **Coal** is actively supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr) and, to some extent, [Eureka Robotics](https://eurekarobotics.com/).

ci/install_linux.sh

-sudo apt-get -qq update
-
-########################
-# Mendatory dependencies
-########################
-sudo apt-get -qq --yes --force-yes install cmake
-sudo apt-get -qq --yes --force-yes install libboost-all-dev
-sudo apt-get -qq --yes --force-yes install libassimp-dev
-sudo apt-get -qq --yes --force-yes install libeigen3-dev
-
-# Octomap
-git clone https://github.com/OctoMap/octomap
-cd octomap
-git checkout tags/v1.8.0
-mkdir build
-cd build
-cmake ..
-make
-sudo make install
-

ci/install_osx.sh

-brew tap homebrew/science
-
-brew install git
-brew install cmake
-brew install boost
-brew install libccd
-brew install assimp
-brew install eigen
-brew install octomap

cmake @ 6b0564f4

-Subproject commit 429af97fe0b858bd73a1747768cc1ac735386140
+Subproject commit 6b0564f45af29a90160aafdfd67eb7a07ace48ed

colcon.pkg

+{
+    "hooks": [
+        "share/hpp-fcl/hook/ament_prefix_path.dsv",
+        "share/hpp-fcl/hook/python_path.dsv"
+    ]
+}
\ No newline at end of file

development/build.md

+# Build and install from source with Pixi
+
+To build Coal from source the easiest way is to use [Pixi](https://pixi.sh/latest/#installation).
+
+[Pixi](https://pixi.sh/latest/) is a cross-platform package management tool for developers that
+will install all required dependencies in `.pixi` directory.
+It's used by our CI agent so you have the guarantee to get the right dependencies.
+
+Run the following command to install dependencies, configure, build and test the project:
+
+```bash
+pixi run test
+```
+
+The project will be built in the `build` directory.
+You can run `pixi shell` and build the project with `cmake` and `ninja` manually.
+

development/release.md

+# Release with Pixi
+
+To create a release with Pixi run the following commands on the **devel** branch:
+
+```bash
+COAL_VERSION=X.Y.Z pixi run release_new_version
+git push origin
+git push origin vX.Y.Z
+git push origin devel:master
+```
+
+Where `X.Y.Z` is the new version.
+Be careful to follow the [Semantic Versioning](https://semver.org/spec/v2.0.0.html) rules.
+
+You will find the following assets:
+- `./build_new_version/coal-X.Y.Z.tar.gz`
+- `./build_new_version/coal-X.Y.Z.tar.gz.sig`
+
+Then, create a new release on [GitHub](https://github.com/coal-library/coal/releases/new) with:
+
+* Tag: vX.Y.Z
+* Title: Coal X.Y.Z
+* Body:
+```
+## What's Changed
+
+CHANGELOG CONTENT
+
+**Full Changelog**: https://github.com/coal-library/coal/compare/vXX.YY.ZZ...vX.Y.Z
+```
+
+Where `XX.YY.ZZ` is the last release version.
+
+Then upload `coal-X.Y.Z.tar.gz` and `coal-X.Y.Z.tar.gz.sig` and publish the release.

development/scripts/pixi/activation.bat

+:: Setup ccache
+set CMAKE_CXX_COMPILER_LAUNCHER=ccache
+
+:: Create compile_commands.json for language server
+set CMAKE_EXPORT_COMPILE_COMMANDS=1
+
+:: Activate color output with Ninja
+set CMAKE_COLOR_DIAGNOSTICS=1
+
+:: Set default build value only if not previously set
+if not defined COAL_BUILD_TYPE (set COAL_BUILD_TYPE=Release)
+if not defined COAL_PYTHON_STUBS (set COAL_PYTHON_STUBS=ON)
+if not defined COAL_HAS_QHULL (set COAL_HAS_QHULL=OFF)

development/scripts/pixi/activation.sh

+#! /bin/bash
+# Activation script
+
+# Remove flags setup from cxx-compiler
+unset CFLAGS
+unset CPPFLAGS
+unset CXXFLAGS
+unset DEBUG_CFLAGS
+unset DEBUG_CPPFLAGS
+unset DEBUG_CXXFLAGS
+unset LDFLAGS
+
+if [[ $host_alias == *"apple"* ]];
+then
+  # On OSX setting the rpath and -L it's important to use the conda libc++ instead of the system one.
+  # If conda-forge use install_name_tool to package some libs, -headerpad_max_install_names is then mandatory
+  export LDFLAGS="-Wl,-headerpad_max_install_names -Wl,-rpath,$CONDA_PREFIX/lib -L$CONDA_PREFIX/lib"
+elif [[ $host_alias == *"linux"* ]];
+then
+  # On GNU/Linux, I don't know if these flags are mandatory with g++ but
+  # it allow to use clang++ as compiler
+  export LDFLAGS="-Wl,-rpath,$CONDA_PREFIX/lib -Wl,-rpath-link,$CONDA_PREFIX/lib -L$CONDA_PREFIX/lib"
+fi
+
+# Setup ccache
+export CMAKE_CXX_COMPILER_LAUNCHER=ccache
+
+# Create compile_commands.json for language server
+export CMAKE_EXPORT_COMPILE_COMMANDS=1
+
+# Activate color output with Ninja
+export CMAKE_COLOR_DIAGNOSTICS=1
+
+# Set default build value only if not previously set
+export COAL_BUILD_TYPE=${COAL_BUILD_TYPE:=Release}
+export COAL_PYTHON_STUBS=${COAL_PYTHON_STUBS:=ON}
+export COAL_HAS_QHULL=${COAL_HAS_QHULL:=OFF}

development/scripts/pixi/activation_clang.sh

+#! /bin/bash
+# Clang activation script
+
+export CC="clang"
+export CXX="clang++"

development/scripts/pixi/activation_clang_cl.bat

+:: Setup clang-cl compiler
+set CC=clang-cl
+set CXX=clang-cl

doc/CMakeLists.txt

+SET(DOXYGEN_XML_OUTPUT          "doxygen-xml"      PARENT_SCOPE)
+SET(DOXYGEN_FILE_PATTERNS       "*.h  *.hh  *.hxx" PARENT_SCOPE)
+SET(DOXYGEN_GENERATE_XML        "YES"              PARENT_SCOPE)
+SET(DOXYGEN_EXPAND_ONLY_PREDEF  "NO"               PARENT_SCOPE)
+SET(DOXYGEN_ENABLE_PREPROCESSING "YES" PARENT_SCOPE)
+SET(DOXYGEN_MACRO_EXPANSION      "YES" PARENT_SCOPE)
+SET(DOXYGEN_EXCLUDE "${PROJECT_SOURCE_DIR}/include/hpp/")
+
+# We must not document octree if Octomap is not setup.
+# This create a build issue when building the bindings because doxygen-autodoc will
+# include octree.h that will include octomap.h.
+IF(NOT COAL_HAS_OCTOMAP)
+  SET(DOXYGEN_EXCLUDE "${DOXYGEN_EXCLUDE} ${PROJECT_SOURCE_DIR}/include/coal/octree.h")
+  SET(DOXYGEN_EXCLUDE "${DOXYGEN_EXCLUDE} ${PROJECT_SOURCE_DIR}/include/coal/serialization/octree.h")
+  SET(DOXYGEN_EXCLUDE "${DOXYGEN_EXCLUDE} ${PROJECT_SOURCE_DIR}/include/coal/internal/traversal_node_octree.h")
+ENDIF()
+SET(DOXYGEN_EXCLUDE ${DOXYGEN_EXCLUDE} PARENT_SCOPE)
+
+SET(DOXYGEN_PREDEFINED          "IS_DOXYGEN"       PARENT_SCOPE)

doc/Doxyfile.extra.in

-FILE_PATTERNS          = *.h  *.hh  *.hxx
-
-GENERATE_TREEVIEW      = NO
+USE_MATHJAX= YES

doc/generate_distance_plot.py

+import matplotlib.pyplot as plt
+import numpy as np
+
+interactive = False
+
+m = 1.0
+b = 1.2
+
+mb = m + b
+
+X = np.array([-mb / 2, 0, m, mb, 2 * mb])
+# X = np.linspace(-1, 4., 21)
+
+
+def dlb(d):
+    if d < 0:
+        return None
+    if d > mb:
+        u = d - mb
+        return mb - m + u / 2
+    return d - m
+
+
+plt.figure(figsize=(9, 3.5))
+# plt.plot(X, X-m, ":k")
+# plt.plot([m+b, X[-1]], [b, b], ":k")
+plt.fill_between(
+    [m + b, X[-1]],
+    [b, b],
+    [b, X[-1] - m],
+    alpha=0.2,
+    hatch="|",
+    facecolor="g",
+    label="Distance lower band area",
+)
+plt.plot(X, [dlb(x) for x in X], "-g", label="distance lower bound")
+# plt.plot([X[0], m, m, X[-1]], [0, 0, b, b], ":k")
+plt.axvspan(X[0], m, alpha=0.5, hatch="\\", facecolor="r", label="Collision area")
+
+
+ax = plt.gca()
+ax.set_xlabel("Object distance")
+ax.set_xticks([0, m, mb])
+ax.set_xticklabels(["0", "security margin", "security margin\n+ break distance"])
+ax.set_yticks([0, b])
+ax.set_yticklabels(["0", "break distance"])
+ax.grid(which="major", ls="solid")
+ax.grid(which="minor", ls="dashed")
+
+plt.axvline(0, ls="solid")
+# plt.axvline(m, ls="dashed", label="margin")
+# plt.axvline(mb, ls="dashed")
+plt.axhline(0.0, ls="solid")
+
+plt.title("Collision and distance lower band")
+plt.legend(loc="lower right")
+if interactive:
+    plt.show()
+else:
+    import os.path as path
+
+    dir_path = path.dirname(path.realpath(__file__))
+    plt.savefig(
+        path.join(dir_path, "distance_computation.png"),
+        bbox_inches="tight",
+        orientation="landscape",
+    )