alexanderfabisch / distance3d Goto Github PK

View Code? Open in Web Editor NEW

50.0 50.0 7.0 123.45 MB

Distance computation and collision detection in 3D.

Home Page: https://alexanderfabisch.github.io/distance3d/

License: Other

Python 100.00%

collision-detection mathematics python

distance3d's People

Contributors

Stargazers

Watchers

Forkers

maartenbehn xzy210 tyoungroy manuel-koch paperwave aries-robot abd-al-qudus

distance3d's Issues

More colliders

Disk support function: radius * [normal[0], normal[1], 0] / ||[normal[0], normal[1], 0]|| (in disk coordinates)
Ellipse support function: radii * norm_vector(radii * normal[:2]) (in ellipse coordinates)
Cone support function: max(disk, [0, 0, height])

Features

Fast AABB tree implementation for broad phase, accelerated with numba #61
Hydroelastic contact model supports Young's modulus as input #62
Script to compute potentials of tetrahedral mesh #56
Add hydroelastic BVH for loading URDFs #68

Get contact points from contact plane

For each pair of colliders we only get the contact plane and depth with GJK+EPA or MPR (GJK+EPA: (closest_point, norm_vector(mtv)); MPR: (position, penetration_direction); indicated by red arrows in the above illustration). Find a way to get the contact shape in the contact plane.

connected to #32

Contact Points in Contact Plane

Overlapping Volumes

Combined Contact Normal

Code

"""
=====
Title
=====
"""
import numpy as np
import pytransform3d.visualizer as pv
import pytransform3d.rotations as pr
import pytransform3d.transformations as pt
from grasp_metrics.hands import MiaHand
from distance3d.broad_phase import BoundingVolumeHierarchy
from distance3d.colliders import Box, MeshGraph
from distance3d.mpr import mpr_penetration
from distance3d import containment_test
from distance3d.utils import plane_basis_from_normal, norm_vector
from distance3d.mesh import make_convex_mesh


hand = MiaHand()
joint_angles = hand.get_grasp_angles("lateral", 0.1)
for joint_name in joint_angles:
    hand.tm_.set_joint(joint_name, joint_angles[joint_name])

bvh = BoundingVolumeHierarchy(hand.tm_, hand.get_base_frame())
bvh.fill_tree_with_colliders(
    hand.tm_, make_artists=True, fill_self_collision_whitelists=True)

object_to_grasp = Box(pt.transform_from(R=np.eye(3), p=np.array([0.04, 0.14, 0.03])),
                      np.array([0.03, 0.025, 0.025]))
object_to_grasp.make_artist()
geometry = object_to_grasp.artist_.geometries[0]
aabb = geometry.get_axis_aligned_bounding_box()


def containment_test_mapping(points, collider):
    collider_type = collider.__class__.__name__
    if collider_type == "Cylinder":
        return containment_test.points_in_cylinder(points, collider.cylinder2origin, collider.radius, collider.length)
    elif collider_type == "Sphere":
        return containment_test.points_in_sphere(points, collider.c, collider.radius)
    elif collider_type == "Box":
        return containment_test.points_in_box(points, collider.box2origin, collider.size)
    elif collider_type == "MeshGraph":
        triangles = make_convex_mesh(collider.vertices)
        return containment_test.points_in_convex_mesh(points, collider.mesh2origin, collider.vertices, triangles)
    # TODO find a better way, attach to collider?
    raise NotImplementedError()


def mesh_volume(vertices, triangles):
    faces = vertices[triangles]
    A = faces[:, 1] - faces[:, 0]
    B = faces[:, 2] - faces[:, 0]
    triangle_normals = np.cross(A, B)

    com = np.mean(vertices, axis=0)
    vertices = vertices - com[np.newaxis]

    total_volume = 0.0
    for triangle_idx in range(len(triangles)):
        verts = vertices[triangles[triangle_idx]]
        triangle_normal = triangle_normals[triangle_idx]
        triangle_center = np.mean(verts, axis=0)
        normal_angle = pr.angle_between_vectors(
            triangle_center, triangle_normal)
        sign = -1.0 if normal_angle > np.pi / 2.0 else 1.0

        J = np.array([
            [verts[0, 0], verts[0, 1], verts[0, 2], 1.0],
            [verts[1, 0], verts[1, 1], verts[1, 2], 1.0],
            [verts[2, 0], verts[2, 1], verts[2, 2], 1.0],
            [0.0, 0.0, 0.0, 1.0],
        ])

        abs_det_J = np.linalg.det(J)

        volume = sign * abs_det_J / 6.0
        total_volume += volume
    return total_volume


fig = pv.figure()
for hand_collider in bvh.get_colliders():
    #hand_collider.artist_.add_artist(fig)
    geometry = hand_collider.artist_.geometries[0]
    #points = geometry.sample_points_poisson_disk(500)
    #fig.add_geometry(points)

contact_volumes = []
for hand_collider in bvh.get_colliders():
    intersection, depth, normal, contact_point = mpr_penetration(
        hand_collider, object_to_grasp)

    if intersection:
        # sample contact volume
        points = np.random.RandomState(5).randn(1000000, 3) * 0.03

        # if you only want to sample in contact plane:
        #points[:, 2] = 0.0

        x, y = plane_basis_from_normal(normal)
        R = np.column_stack((x, y, normal))
        points_in_world = points.dot(R.T) + contact_point

        contained_in_hand = containment_test_mapping(points_in_world, hand_collider)
        contained_in_object = containment_test_mapping(points_in_world, object_to_grasp)
        contained = np.logical_and(contained_in_hand, contained_in_object)
        contact_points = points_in_world[contained]
        if len(contact_points) == 0:
            continue

        # show overlapping mesh (always convex(?))
        triangles = make_convex_mesh(contact_points)
        convex_mesh = MeshGraph(np.eye(4), contact_points, triangles)
        convex_mesh.make_artist(c=(1, 0, 0))
        convex_mesh.artist_.add_artist(fig)

        #volume = mesh_volume(contact_points, triangles)
        contact_volumes.append((depth, normal))

        # show samples:
        #fig.scatter(contact_points, s=0.0005, c=(0, 0, 0))

        fig.plot_vector(contact_point, depth * normal, c=(1, 0, 0))

assert len(contact_volumes) > 0
weights = np.array([v[0] for v in contact_volumes])
weights /= sum(weights)
normals = np.array([v[1] for v in contact_volumes])
contact_normal = norm_vector(weights.dot(normals))
fig.plot_vector(object_to_grasp.center(), 0.01 * contact_normal, c=(0, 0, 1))

fig.add_geometry(aabb)
fig.show()

Move cached vertices from colliders to GJK / EPA

Implement Minkowski Portal Refinement

Unit tests

Release 0.6.0

Features

Ellipse collider #34
GJK from Jolt Physics #38
- GJK intersection test is about 30-40% faster
- GJK distance calculation is about 3-5x faster
Adds distance3d.geometry.barycentric_coordinates_tetrahedron
Adds hydroelastic contact model (module: distance3d.hydroelastic_contact) #44
Adds function to load tetrahedral meshes: distance3d.io.load_tetrahedral_mesh

Unable to install version 0.8, nested dependency open3d fails to install

I'm trying to setup a new python environment and installing distance3d doesn't work for me anymore.
I was using distance3d earlier with the same version range (>=0.8,<1.0) on my computer successfully.
Is open3d a new (mandatory) requirement ?

Using the following simple project definition fails for me, running on macOS 14.3.1 (23D60), arm64:

project.toml

[build-system]
requires = ["hatchling"]
build-backend = "hatchling.build"

[project]
name = "foo"
version = "0.0.0"
description = 'Foo'
requires-python = ">=3.10"
license = { text = "ISC" }
keywords = []
authors = []
classifiers = [
  "Programming Language :: Python",
  "Programming Language :: Python :: 3.10",
]
dependencies = [
  "numpy>=1,<2",
  "distance3d>=0.8,<1.0",
]

[project.urls]
Documentation = "https://foo.com"
Source = "https://foo.com"
Changelog = "https://foo.com"
Issues = ""

[tool.hatch.build.targets.sdist]
# using just a directory "python_foo" with an empty file "__init__.py"
packages = ["python_foo"]

[tool.hatch.build.targets.wheel]
packages = ["python_foo"]

[tool.hatch.envs.default]
dependencies = []

Trying to setup the environment fails:

$ hatch env prune ; hatch --verbose env create

Finished creating environment: default
Obtaining file:///Users/manuelkoch/tmp/test-env
  Installing build dependencies: started
  Installing build dependencies: finished with status 'done'
  Checking if build backend supports build_editable: started
  Checking if build backend supports build_editable: finished with status 'done'
  Getting requirements to build editable: started
  Getting requirements to build editable: finished with status 'done'
  Preparing editable metadata (pyproject.toml): started
  Preparing editable metadata (pyproject.toml): finished with status 'done'
Collecting distance3d<1.0,>=0.8 (from foo==0.0.0)
  Using cached distance3d-0.8.0.tar.gz (78 kB)
  Installing build dependencies: started
  Installing build dependencies: finished with status 'done'
  Getting requirements to build wheel: started
  Getting requirements to build wheel: finished with status 'done'
  Preparing metadata (pyproject.toml): started
  Preparing metadata (pyproject.toml): finished with status 'done'
Collecting numpy<2,>=1 (from foo==0.0.0)
  Using cached numpy-1.26.4-cp312-cp312-macosx_11_0_arm64.whl.metadata (61 kB)
Collecting scipy (from distance3d<1.0,>=0.8->foo==0.0.0)
  Using cached scipy-1.12.0-cp312-cp312-macosx_12_0_arm64.whl.metadata (217 kB)
Collecting matplotlib (from distance3d<1.0,>=0.8->foo==0.0.0)
  Using cached matplotlib-3.8.3-cp312-cp312-macosx_11_0_arm64.whl.metadata (5.8 kB)
Collecting pytransform3d (from distance3d<1.0,>=0.8->foo==0.0.0)
  Using cached pytransform3d-3.5.0.tar.gz (102 kB)
  Installing build dependencies: started
  Installing build dependencies: finished with status 'done'
  Getting requirements to build wheel: started
  Getting requirements to build wheel: finished with status 'done'
  Preparing metadata (pyproject.toml): started
  Preparing metadata (pyproject.toml): finished with status 'done'
INFO: pip is looking at multiple versions of distance3d to determine which version is compatible with other requirements. This could take a while.
ERROR: Ignored the following versions that require a different python version: 1.10.0 Requires-Python <3.12,>=3.8; 1.10.0rc1 Requires-Python <3.12,>=3.8; 1.10.0rc2 Requires-Python <3.12,>=3.8; 1.10.1 Requires-Python <3.12,>=3.8; 1.21.2 Requires-Python >=3.7,<3.11; 1.21.3 Requires-Python >=3.7,<3.11; 1.21.4 Requires-Python >=3.7,<3.11; 1.21.5 Requires-Python >=3.7,<3.11; 1.21.6 Requires-Python >=3.7,<3.11; 1.6.2 Requires-Python >=3.7,<3.10; 1.6.3 Requires-Python >=3.7,<3.10; 1.7.0 Requires-Python >=3.7,<3.10; 1.7.1 Requires-Python >=3.7,<3.10; 1.7.2 Requires-Python >=3.7,<3.11; 1.7.3 Requires-Python >=3.7,<3.11; 1.8.0 Requires-Python >=3.8,<3.11; 1.8.0rc1 Requires-Python >=3.8,<3.11; 1.8.0rc2 Requires-Python >=3.8,<3.11; 1.8.0rc3 Requires-Python >=3.8,<3.11; 1.8.0rc4 Requires-Python >=3.8,<3.11; 1.8.1 Requires-Python >=3.8,<3.11; 1.9.0 Requires-Python >=3.8,<3.12; 1.9.0rc1 Requires-Python >=3.8,<3.12; 1.9.0rc2 Requires-Python >=3.8,<3.12; 1.9.0rc3 Requires-Python >=3.8,<3.12; 1.9.1 Requires-Python >=3.8,<3.12
ERROR: Could not find a version that satisfies the requirement open3d (from distance3d) (from versions: none)
ERROR: No matching distribution found for open3d

My project wants to use Python 3.10. I don't see why it couldn't find a matching open3d version given the previous pip output.
All of the following versions seem to be compatible with my Python interpreter ( as far as details in the pip output are stating ):

1.10.0 Requires-Python <3.12,>=3.8
1.10.0rc1 Requires-Python <3.12,>=3.8
1.10.0rc2 Requires-Python <3.12,>=3.8
1.10.1 Requires-Python <3.12,>=3.8
1.21.2 Requires-Python >=3.7,<3.11
1.21.3 Requires-Python >=3.7,<3.11
1.21.4 Requires-Python >=3.7,<3.11
1.21.5 Requires-Python >=3.7,<3.11
1.21.6 Requires-Python >=3.7,<3.11
1.7.2 Requires-Python >=3.7,<3.11
1.7.3 Requires-Python >=3.7,<3.11
1.8.0 Requires-Python >=3.8,<3.11
1.8.0rc1 Requires-Python >=3.8,<3.11
1.8.0rc2 Requires-Python >=3.8,<3.11
1.8.0rc3 Requires-Python >=3.8,<3.11
1.8.0rc4 Requires-Python >=3.8,<3.11
1.8.1 Requires-Python >=3.8,<3.11
1.9.0 Requires-Python >=3.8,<3.12
1.9.0rc1 Requires-Python >=3.8,<3.12
1.9.0rc2 Requires-Python >=3.8,<3.12
1.9.0rc3 Requires-Python >=3.8,<3.12
1.9.1 Requires-Python >=3.8,<3.12

Return and reuse separating axis

GJK improvements

mpr.mpr_penetration stuck in endless loop for two meshes

The following code for two meshes ( one is completely covered by the other ) is stuck in endless loop:

Using:

MacOS Ventura 13.4.1 (22F82) on M1 arm64
Python 3.10.6
distance3d from master branch commit dede764
numba 0.57.0
numpy 1.24.3

from pyrr import Vector3
from trimesh import Scene, Trimesh

vertices_a = numpy.array(
    [
        Vector3([23.69677426, 1310.32310771, 552.84175763]),
        Vector3([16.30322573, 1310.32310771, 552.84175763]),
        Vector3([12.17142738, 1310.3231053, 548.7099655]),
        Vector3([12.17142738, 1310.3231053, 541.2900345]),
        Vector3([16.30322573, 1310.32310771, 537.15824237]),
        Vector3([23.69677426, 1310.32310771, 537.15824237]),
        Vector3([27.82857167, 1310.32310531, 541.2900345]),
        Vector3([27.82857167, 1310.32310531, 548.7099655]),
        Vector3([23.25910977, 1310.56902972, 552.83423821]),
        Vector3([16.74089023, 1310.56902972, 552.83423821]),
        Vector3([12.178948, 1310.56842155, 548.27298778]),
        Vector3([12.178948, 1310.56842155, 541.72701222]),
        Vector3([23.25910977, 1310.56902972, 537.16576179]),
        Vector3([16.74089023, 1310.56902972, 537.16576179]),
        Vector3([27.82105104, 1310.56842156, 541.72701222]),
        Vector3([27.82105104, 1310.56842156, 548.27298778]),
        Vector3([16.75429825, 1315.14605025, 546.36398605]),
        Vector3([18.64989135, 1315.14605026, 548.25954888]),
        Vector3([21.35010865, 1315.14605026, 548.25954888]),
        Vector3([23.24570175, 1315.14605025, 546.36398605]),
        Vector3([23.24570175, 1315.14605025, 543.63601395]),
        Vector3([21.35010865, 1315.14605026, 541.74045112]),
        Vector3([18.64989135, 1315.14605026, 541.74045112]),
        Vector3([16.75429825, 1315.14605025, 543.63601395]),
    ],
    dtype=numpy.float64,
)
faces_a = numpy.array(
    [
        [1, 16, 2],
        [16, 1, 17],
        [17, 1, 9],
        [2, 16, 10],
        [7, 19, 0],
        [7, 15, 19],
        [0, 19, 18],
        [20, 14, 6],
        [5, 20, 6],
        [21, 20, 5],
        [5, 12, 21],
        [4, 3, 23],
        [23, 22, 4],
        [23, 3, 11],
        [4, 22, 13],
        [0, 18, 8],
        [10, 3, 2],
        [11, 3, 10],
        [14, 7, 6],
        [15, 7, 14],
        [15, 14, 20],
        [19, 15, 20],
        [2, 3, 5],
        [5, 1, 2],
        [6, 7, 5],
        [0, 1, 5],
        [5, 3, 4],
        [5, 7, 0],
        [23, 18, 19],
        [16, 17, 23],
        [19, 20, 23],
        [21, 22, 23],
        [17, 18, 23],
        [23, 20, 21],
        [11, 10, 23],
        [23, 10, 16],
        [13, 5, 4],
        [12, 5, 13],
        [13, 22, 21],
        [21, 12, 13],
        [8, 1, 0],
        [9, 1, 8],
        [8, 18, 17],
        [8, 17, 9],
    ],
)
vertices_b = numpy.array(
    [
        Vector3([23.70402525, 1310.25599226, 552.8347335]),
        Vector3([16.29597475, 1310.25599226, 552.8347335]),
        Vector3([12.17844916, 1310.25599021, 548.71721605]),
        Vector3([12.17844916, 1310.25599021, 541.28278395]),
        Vector3([16.29597475, 1310.25599226, 537.1652665]),
        Vector3([23.70402525, 1310.25599226, 537.1652665]),
        Vector3([27.82154988, 1310.25599022, 541.28278395]),
        Vector3([27.82154988, 1310.25599022, 548.71721605]),
        Vector3([23.25910977, 1310.56902972, 552.83423821]),
        Vector3([16.74089023, 1310.56902972, 552.83423821]),
        Vector3([12.178948, 1310.56842155, 548.27298778]),
        Vector3([12.178948, 1310.56842155, 541.72701222]),
        Vector3([23.25910977, 1310.56902972, 537.16576179]),
        Vector3([16.74089023, 1310.56902972, 537.16576179]),
        Vector3([27.82105104, 1310.56842156, 541.72701222]),
        Vector3([27.82105104, 1310.56842156, 548.27298778]),
        Vector3([16.75429825, 1315.14605025, 546.36398605]),
        Vector3([18.64989135, 1315.14605026, 548.25954888]),
        Vector3([21.35010865, 1315.14605026, 548.25954888]),
        Vector3([23.24570175, 1315.14605025, 546.36398605]),
        Vector3([23.24570175, 1315.14605025, 543.63601395]),
        Vector3([21.35010865, 1315.14605026, 541.74045112]),
        Vector3([18.64989135, 1315.14605026, 541.74045112]),
        Vector3([16.75429825, 1315.14605025, 543.63601395]),
        Vector3([23.71710962, 181.69901258, 552.84825455]),
        Vector3([16.28289038, 181.69901258, 552.84825455]),
        Vector3([12.16492808, 181.69901257, 548.73029888]),
        Vector3([12.16492808, 181.69901257, 541.26970112]),
        Vector3([16.28289038, 181.69901258, 537.15174545]),
        Vector3([23.71710962, 181.69901258, 537.15174545]),
        Vector3([27.83507097, 181.69901257, 541.26970112]),
        Vector3([27.83507097, 181.69901257, 548.73029888]),
    ],
    dtype=numpy.float64,
)
faces_b = numpy.array(
    [
        [1, 16, 2],
        [16, 1, 17],
        [17, 1, 9],
        [2, 16, 10],
        [7, 19, 0],
        [7, 15, 19],
        [0, 19, 18],
        [20, 14, 6],
        [5, 20, 6],
        [21, 20, 5],
        [5, 12, 21],
        [4, 3, 23],
        [23, 22, 4],
        [23, 3, 11],
        [4, 22, 13],
        [0, 18, 8],
        [10, 3, 2],
        [11, 3, 10],
        [14, 7, 6],
        [15, 7, 14],
        [15, 14, 20],
        [19, 15, 20],
        [23, 18, 19],
        [16, 17, 23],
        [19, 20, 23],
        [21, 22, 23],
        [17, 18, 23],
        [23, 20, 21],
        [11, 10, 23],
        [23, 10, 16],
        [13, 5, 4],
        [12, 5, 13],
        [13, 22, 21],
        [21, 12, 13],
        [8, 1, 0],
        [9, 1, 8],
        [8, 18, 17],
        [8, 17, 9],
        [26, 27, 29],
        [29, 25, 26],
        [30, 31, 29],
        [24, 25, 29],
        [29, 27, 28],
        [29, 31, 24],
        [25, 24, 1],
        [1, 24, 0],
        [26, 25, 2],
        [2, 25, 1],
        [27, 26, 3],
        [3, 26, 2],
        [28, 27, 4],
        [4, 27, 3],
        [29, 28, 5],
        [5, 28, 4],
        [30, 29, 6],
        [6, 29, 5],
        [24, 31, 0],
        [0, 31, 7],
        [31, 30, 7],
        [7, 30, 6],
    ],
)

scene = Scene()
mesh_a = Trimesh(vertices=vertices_a, faces=faces_a)
mesh_a.visual.face_colors = (255, 0, 0, 80)
scene.add_geometry(mesh_a)
mesh_b = Trimesh(vertices=vertices_b, faces=faces_b)
mesh_b.visual.face_colors = (0, 255, 0, 80)
scene.add_geometry(mesh_b)
scene.show()

collider_a = colliders.ConvexHullVertices(vertices_a)
collider_b = colliders.ConvexHullVertices(vertices_b)
# call to mpr.mpr_penetration gets stuck in endless loop
colliding, penetration_depth, penetration_dir, contact_position = mpr.mpr_penetration(
    collider_a, collider_b
)
print(f"A vs B: colliding={colliding} penetration_depth={penetration_depth}")

Scene looks like

Scene from another view point

Release 0.2.0

Features

Self collision detection for robots
Distance computation
- from line to plane
- from line segment to plane
- from line to circle
- from disk to disk

Distance between plane and collider that has support function

Release 0.7.0

Features

Improvements of hydroelastic contacts
- Faster tetrahedron intersections with AABB checks
- Tetrahedral box meshes #50
- Tetrahedral ellipsoid meshes #51
- Tetrahedral cylinder meshes #52
- Tetrahedral capsule meshes #53

Release 0.9.0

Features

Add Nesterov-accelerated GJK

Bugfixes

Fix numerical problem in Jolt's GJK implementation.
Fix numerical problem in MPR.
Use time.perf_counter() for benchmark.

Pass numba jit-compiled support functions to GJK, EPA, and MPR instead of colliders

Example:

import numba
from numba.experimental import jitclass


spec = {
    "ellipsoid2origin": numba.float64[:, ::1],
    "radii": numba.float64[::1]
}


@jitclass(spec)
class EllipsoidSupportFunction:
    def __init__(self, ellipsoid2origin, radii):
        self.ellipsoid2origin = ellipsoid2origin
        self.radii = radii

    def support_function(self, search_direction):
        return geometry.support_function_ellipsoid(
            search_direction, self.ellipsoid2origin, self.radii)

Speed up GJK distance calculation with numba

Benchmark distance3d vs. PyBullet

Nesterov-accelerated GJK

Documentation

Two simple cylinder mesh cause assertion error in gjk function

I see assertion errors with rather simple geometries.

Using:

MacOS Ventura 13.4.1 (22F82) on M1 arm64
Python 3.10.6
distance3d from master branch commit dede764
numba 0.57.0
numpy 1.24.3

E.g. here are two cylinders, that are in collision:

from pyrr import Vector3

vertices_a = numpy.array(
    [
        Vector3([1.7521845e01, 7.4650002e01, -2.7000427e-02], dtype=numpy.float32),
        Vector3([1.2424170e01, 7.4650002e01, -2.7000427e-02], dtype=numpy.float32),
        Vector3([1.2424170e01, 9.9465002e02, -2.7000427e-02], dtype=numpy.float32),
        Vector3([1.7521845e01, 9.9465002e02, -2.7000427e-02], dtype=numpy.float32),
        Vector3([29.973, 74.65, 17.956755], dtype=numpy.float32),
        Vector3([29.973, 74.65, 12.424185], dtype=numpy.float32),
        Vector3([29.973, 994.65, 12.424185], dtype=numpy.float32),
        Vector3([29.973, 994.65, 17.956755], dtype=numpy.float32),
        Vector3([17.956755, 74.65, 29.973], dtype=numpy.float32),
        Vector3([12.42417, 74.65, 29.973], dtype=numpy.float32),
        Vector3([6.7186646, 74.65, 27.93186], dtype=numpy.float32),
        Vector3([2.0141246, 74.65, 23.22732], dtype=numpy.float32),
        Vector3([-2.7000427e-02, 7.4650002e01, 1.7521845e01], dtype=numpy.float32),
        Vector3([-2.7000427e-02, 7.4650002e01, 1.1989244e01], dtype=numpy.float32),
        Vector3([2.2566445, 74.65, 6.4761295], dtype=numpy.float32),
        Vector3([6.7186494, 74.65, 2.0141397], dtype=numpy.float32),
        Vector3([23.22732, 74.65, 2.0141246], dtype=numpy.float32),
        Vector3([27.93186, 74.65, 6.7186646], dtype=numpy.float32),
        Vector3([27.689354, 74.65, 23.46987], dtype=numpy.float32),
        Vector3([23.46987, 74.65, 27.689354], dtype=numpy.float32),
        Vector3([-2.7000427e-02, 9.9465002e02, 1.7521845e01], dtype=numpy.float32),
        Vector3([-2.7000427e-02, 9.9465002e02, 1.1989244e01], dtype=numpy.float32),
        Vector3([12.42417, 994.65, 29.973], dtype=numpy.float32),
        Vector3([17.956755, 994.65, 29.973], dtype=numpy.float32),
        Vector3([23.46987, 994.65, 27.689354], dtype=numpy.float32),
        Vector3([27.689354, 994.65, 23.46987], dtype=numpy.float32),
        Vector3([27.93186, 994.65, 6.7186646], dtype=numpy.float32),
        Vector3([23.22732, 994.65, 2.0141246], dtype=numpy.float32),
        Vector3([6.7186494, 994.65, 2.0141397], dtype=numpy.float32),
        Vector3([2.2566445, 994.65, 6.4761295], dtype=numpy.float32),
        Vector3([2.0141246, 994.65, 23.22732], dtype=numpy.float32),
        Vector3([6.7186646, 994.65, 27.93186], dtype=numpy.float32),
    ],
    dtype=numpy.float64,
)
vertices_b = numpy.array(
    [
        Vector3([16.861032, 99.65, 26.08411], dtype=numpy.float32),
        Vector3([13.084977, 99.65, 26.08411], dtype=numpy.float32),
        Vector3([13.084977, 54.65, 26.08411], dtype=numpy.float32),
        Vector3([16.861032, 54.65, 26.08411], dtype=numpy.float32),
        Vector3([26.08411, 99.65, 12.762811], dtype=numpy.float32),
        Vector3([26.08411, 99.65, 16.861012], dtype=numpy.float32),
        Vector3([26.08411, 54.65, 16.861012], dtype=numpy.float32),
        Vector3([26.08411, 54.65, 12.762811], dtype=numpy.float32),
        Vector3([17.18319, 99.65, 3.8618884], dtype=numpy.float32),
        Vector3([13.084977, 99.65, 3.8618884], dtype=numpy.float32),
        Vector3([8.858677, 99.65, 5.373844], dtype=numpy.float32),
        Vector3([5.3738327, 99.65, 8.858688], dtype=numpy.float32),
        Vector3([3.8618884, 99.65, 13.084967], dtype=numpy.float32),
        Vector3([3.8618884, 99.65, 17.18319], dtype=numpy.float32),
        Vector3([5.5534773, 99.65, 21.266977], dtype=numpy.float32),
        Vector3([8.858666, 99.65, 24.572155], dtype=numpy.float32),
        Vector3([21.08731, 99.65, 24.572166], dtype=numpy.float32),
        Vector3([24.572155, 99.65, 21.087322], dtype=numpy.float32),
        Vector3([24.39252, 99.65, 8.679022], dtype=numpy.float32),
        Vector3([21.266977, 99.65, 5.5534773], dtype=numpy.float32),
        Vector3([3.8618884, 54.65, 13.084967], dtype=numpy.float32),
        Vector3([3.8618884, 54.65, 17.18319], dtype=numpy.float32),
        Vector3([13.084977, 54.65, 3.8618884], dtype=numpy.float32),
        Vector3([17.18319, 54.65, 3.8618884], dtype=numpy.float32),
        Vector3([21.266977, 54.65, 5.5534773], dtype=numpy.float32),
        Vector3([24.39252, 54.65, 8.679022], dtype=numpy.float32),
        Vector3([24.572155, 54.65, 21.087322], dtype=numpy.float32),
        Vector3([21.08731, 54.65, 24.572166], dtype=numpy.float32),
        Vector3([8.858666, 54.65, 24.572155], dtype=numpy.float32),
        Vector3([5.5534773, 54.65, 21.266977], dtype=numpy.float32),
        Vector3([5.3738327, 54.65, 8.858688], dtype=numpy.float32),
        Vector3([8.858677, 54.65, 5.373844], dtype=numpy.float32),
    ],
    dtype=numpy.float64,
)

collider_a = colliders.ConvexHullVertices(vertices_a)
collider_b = colliders.ConvexHullVertices(vertices_b)
distance, closest_point_a, closest_point_b, simplex = gjk.gjk(collider_a, collider_b)
colliding = numpy.allclose(distance, 0)
penetration_depth = 0.0
if colliding:
    minimum_translation_vector, minkowski_faces, success = epa.epa(
        simplex,
        collider_a,
        collider_b,
        max_faces=512,
    )
    if not numpy.allclose(minimum_translation_vector, 0):
        penetration_depth = numpy.linalg.norm(minimum_translation_vector)
print(colliding, penetration_depth)

which throws

  File "python/src/distance3d/distance3d/gjk/_gjk_jolt.py", line 213, in gjk_distance_jolt
    assert abs(np.dot(search_direction, search_direction) - v_len_sq) < 1e-12
AssertionError

Two trivial overlapping boxes trigger assertion in gjk

I'm struggling with an issue when calculating the distance between two overlapping boxes, i.e. one tall box overlaps another box largely.
The following snippet triggers an assertion:

import numpy
from distance3d import colliders
from distance3d import gjk

if __name__ == "__main__":
    vertices_a = numpy.array(
        [
            [-119.5, -120.5, -119.5],
            [-119.5, -119.5, -119.5],
            [-119.5, -120.5, -120.5],
            [-119.5, -119.5, -120.5],
            [-89.5, -120.5, -119.5],
            [-89.5, -119.5, -119.5],
            [-89.5, -120.5, -120.5],
            [-89.5, -119.5, -120.5],
        ],
        dtype=numpy.float64,
    )
    vertices_b = numpy.array(
        [
            [-90.0, -120.14644661, -120.85355335],
            [-90.0, -120.85355339, -120.14644656],
            [-90.70710679, -119.64644658, -120.35355338],
            [-90.70710679, -120.35355336, -119.64644659],
            [-89.29289321, -119.64644664, -120.35355341],
            [-89.29289321, -120.35355342, -119.64644662],
            [-90.0, -119.14644661, -119.85355344],
            [-90.0, -119.85355339, -119.14644665],
        ],
        dtype=numpy.float64,
    )

    collider_a = colliders.ConvexHullVertices(vertices_a)
    collider_b = colliders.ConvexHullVertices(vertices_b)
    distance, closest_point_a, closest_point_b, simplex = gjk.gjk(collider_a, collider_b)
    print(distance, closest_point_a, closest_point_b, simplex)

Traceback (most recent call last):
  File "distance3d_sandboy.py", line 42, in <module>
    distance, closest_point_a, closest_point_b, simplex = gjk.gjk(collider_a, collider_b)
  File "python/site-packages/distance3d/gjk/_gjk_jolt.py", line 213, in gjk_distance_jolt
    assert abs(np.dot(search_direction, search_direction) - v_len_sq) < EPSILON
AssertionError

Debugger shows values:
search_direction=[-5.95073547e-17, -2.07708528e-08, -2.07708527e-08]
v_len_sq=0.0

Any idea how to avoid such assertion ?

My environment is

MacOS Ventura 13.3.1 (22E261) arm64
Python 3.10.6
distance3d 0.7.1
numba 0.57.0
numpy 1.24.3

Release 0.4.0

Features

Add support function for box and sphere
#24
#14
#15
#18, about 20x faster for collision detection than GJK

Performance

#13 , for example:
- point_to_triangle is 7x faster
- point_to_plane is 2.5x faster
- point_to_disk is 6x faster
- line_to_line_segment is 6x faster
- line_segment_to_line_segment is 5x faster
- line_segment_to_plane is 5x faster
- line_to_triangle is 15x faster
- line_segment_to_triangle is 14x faster
#26
- 20x faster than standard GJK and 10% faster than MPR

Refactoring

#11
#23
All support functions have name prefix support_function_
Move BoundingVolumeHierarchy to new module broad_phase
Rename Convex to ConvexHullVertices

GJK error

import numpy as np
from numpy import array
from distance3d import colliders, gjk, mesh
import pytransform3d.visualizer as pv


vertices1=array([[1.9408982677575293  , 1.7555630101186028  , 0.18558370457138618 ],
                    [0.8325772811802072  , 0.07348739881751665 , 1.7363835855830982  ],
                    [0.4647979626331866  , 0.7802364656205514  , 0.7683352436509485  ],
                    [1.8147849498111688  , 0.1710650007346617  , 1.802196858095821   ],
                    [0.05742372721610556 , 1.6599005060005285  , 0.014474511746654706],
                    [1.5270861966500002  , 0.4945624540164677  , 0.7162419894551117  ]])
vertices2=array([[0.13746727993941066 , 0.20129636190312772 , 0.34482818309629215 ],
                    [0.5315223185064999  , 0.859293459032485   , 0.6275795914219559  ],
                    [0.5810087058274663  , 0.42871589938255994 , 0.6201313431197037  ],
                    [0.48395328848552066 , 0.35710287272501984 , 0.27586601014041534 ],
                    [0.1586377710691551  , 0.6421620047252284  , 0.6487788989727401  ],
                    [0.5185197562895357  , 0.058044153658207476, 0.4020269780737239  ]])


#convex1 = colliders.Convex(vertices1)
#convex2 = colliders.Convex(vertices2)
triangles1 = mesh.make_convex_mesh(vertices1)
convex1 = colliders.MeshGraph(np.eye(4), vertices1, triangles1)
triangles2 = mesh.make_convex_mesh(vertices2)
convex2 = colliders.MeshGraph(np.eye(4), vertices2, triangles2)
print(gjk.gjk_intersection(convex1, convex2))
print(gjk.gjk(convex1, convex2))
fig = pv.figure()
convex1.make_artist((1, 0, 0))
convex2.make_artist((0, 1, 0))
convex1.artist_.add_artist(fig)
convex2.artist_.add_artist(fig)
fig.show()

Possible solution:

-        return self.d[1, 2] <= 0.0
+        return self.d[1, 2] <= EPSILON
 
     def line_segment_01_of_line_segment_optimal(self):
         return not (self.vertex_1_of_line_segment_optimal() or self.vertex_0_of_line_segment_optimal())
 
     def vertex_1_of_line_segment_optimal(self):
-        return self.d[0, 2] <= 0.0
+        return self.d[0, 2] <= EPSILON
 
     def vertex_0_of_face_optimal(self):
-        return not (self.d[1, 2] > 0.0 or self.d[2, 4] > 0.0)
+        return not (self.d[1, 2] > -EPSILON or self.d[2, 4] > -EPSILON)
 
     def line_segment_01_of_face_optimal(self):
-        return self.line_segment_01_of_line_segment_optimal() and not self.d[2, 6] > 0.0
+        return self.line_segment_01_of_line_segment_optimal() and not self.d[2, 6] > -EPSILON
 
     def line_segment_02_of_face_optimal(self):
-        return not (self.d[0, 4] <= 0.0 or self.d[1, 6] > 0.0 or self.d[2, 4] <= 0.0)
+        return not (self.d[0, 4] <= EPSILON or self.d[1, 6] > -EPSILON or self.d[2, 4] <= EPSILON)
 
     def face_012_of_face_optimal(self):
-        return not (self.d[0, 6] <= 0.0 or self.d[1, 6] <= 0.0 or self.d[2, 6] <= 0.0)
+        return not (self.d[0, 6] <= EPSILON or self.d[1, 6] <= EPSILON or self.d[2, 6] <= EPSILON)
 
     def vertex_1_of_face_optimal(self):
-        return not (self.d[0, 2] > 0.0 or self.d[2, 5] > 0.0)
+        return not (self.d[0, 2] > -EPSILON or self.d[2, 5] > -EPSILON)
 
     def vertex_2_of_face_optimal(self):
-        return not (self.d[0, 4] > 0.0 or self.d[1, 5] > 0.0)
+        return not (self.d[0, 4] > -EPSILON or self.d[1, 5] > -EPSILON)
 
     def line_segment_12_of_face_optimal(self):
-        return not self.d[0, 6] > 0.0 and self.check_line_segment_12_of_face()
+        return not self.d[0, 6] > -EPSILON and self.check_line_segment_12_of_face()
 
     def vertex_0_of_tetrahedron_optimal(self):
-        return self.vertex_0_of_face_optimal() and not self.d[3, 8] > 0.0
+        return self.vertex_0_of_face_optimal() and not self.d[3, 8] > -EPSILON
 
     def line_segment_01_of_tetrahedron_optimal(self):
-        return self.line_segment_01_of_face_optimal() and not self.d[3, 11] > 0.0
+        return self.line_segment_01_of_face_optimal() and not self.d[3, 11] > -EPSILON
 
     def line_segment_02_of_tetrahedron_optimal(self):
-        return self.line_segment_02_of_face_optimal() and not self.d[3, 12] > 0.0
+        return self.line_segment_02_of_face_optimal() and not self.d[3, 12] > -EPSILON
 
     def face_012_of_tetrahedron_optimal(self):
-        return self.face_012_of_face_optimal() and not self.d[3, 14] > 0.0
+        return self.face_012_of_face_optimal() and not self.d[3, 14] > -EPSILON
 
     def line_segment_03_of_tetrahedron_optimal(self):
-        return not (self.d[1, 11] > 0.0 or self.d[2, 12] > 0.0) and self.check_line_segment_03_of_tetrahedron()
+        return not (self.d[1, 11] > -EPSILON or self.d[2, 12] > -EPSILON) and self.check_line_segment_03_of_tetrahedron()
 
     def face_013_of_tetrahedron_optimal(self):
-        return not self.d[2, 14] > 0.0 and self.check_face_013_of_tetrahedron()
+        return not self.d[2, 14] > -EPSILON and self.check_face_013_of_tetrahedron()
 
     def face_023_of_tetrahedron_optimal(self):
-        return not self.d[1, 14] > 0.0 and self.check_face_023_of_tetrahedron()
+        return not self.d[1, 14] > -EPSILON and self.check_face_023_of_tetrahedron()
 
     def convex_hull_of_tetrahedron_optimal(self):
-        return not (self.d[0, 14] <= 0.0 or self.d[1, 14] <= 0.0 or self.d[2, 14] <= 0.0 or self.d[3, 14] <= 0.0)
+        return not (self.d[0, 14] <= EPSILON or self.d[1, 14] <= EPSILON or self.d[2, 14] <= EPSILON or self.d[3, 14] <= EPSILON)
 
     def vertex_1_of_tetrahedron_optimal(self):
-        return self.vertex_1_of_face_optimal() and not self.d[3, 9] > 0.0
+        return self.vertex_1_of_face_optimal() and not self.d[3, 9] > -EPSILON
 
     def vertex_2_of_tetrahedron_optimal(self):
-        return self.vertex_2_of_face_optimal() and not self.d[3, 10] > 0.0
+        return self.vertex_2_of_face_optimal() and not self.d[3, 10] > -EPSILON
 
     def vertex_3_of_tetrahedron_optimal(self):
-        return not (self.d[0, 8] > 0.0 or self.d[1, 9] > 0.0 or self.d[2, 10] > 0.0)
+        return not (self.d[0, 8] > -EPSILON or self.d[1, 9] > -EPSILON or self.d[2, 10] > -EPSILON)
 
     def line_segment_12_of_tetrahedron_optimal(self):
-        return self.line_segment_12_of_face_optimal() and not self.d[3, 13] > 0.0
+        return self.line_segment_12_of_face_optimal() and not self.d[3, 13] > -EPSILON
 
     def line_segment_13_of_tetrahedron_optimal(self):
-        return not (self.d[0, 11] > 0.0 or self.d[2, 13] > 0.0) and self.check_line_segment_13_of_tetrahedron()
+        return not (self.d[0, 11] > -EPSILON or self.d[2, 13] > -EPSILON) and self.check_line_segment_13_of_tetrahedron()
 
     def line_segment_23_of_tetrahedron_optimal(self):
-        return not (self.d[0, 12] > 0.0 or self.d[1, 13] > 0.0) and self.check_line_segment_23_of_tetrahedron()
+        return not (self.d[0, 12] > -EPSILON or self.d[1, 13] > -EPSILON) and self.check_line_segment_23_of_tetrahedron()
 
     def face_123_of_tetrahedron_optimal(self):
-        return not self.d[0, 14] > 0.0 and self.check_face_123_of_tetrahedron()
+        return not self.d[0, 14] > -EPSILON and self.check_face_123_of_tetrahedron()
 
     def check_line_segment_02_of_face(self):
-        return not (self.d[0, 4] <= 0.0 or self.d[2, 4] <= 0.0)
+        return not (self.d[0, 4] <= EPSILON or self.d[2, 4] <= EPSILON)
 
     def check_face_012_of_face(self):
-        return not (self.d[0, 6] <= 0.0 or self.d[1, 6] <= 0.0 or self.d[2, 6] <= 0.0)
+        return not (self.d[0, 6] <= EPSILON or self.d[1, 6] <= EPSILON or self.d[2, 6] <= EPSILON)
 
     def check_line_segment_12_of_face(self):
-        return not (self.d[1, 5] <= 0.0 or self.d[2, 5] <= 0.0)
+        return not (self.d[1, 5] <= EPSILON or self.d[2, 5] <= EPSILON)
 
     def check_line_segment_03_of_tetrahedron(self):
-        return not (self.d[0, 8] <= 0.0 or self.d[3, 8] <= 0.0)
+        return not (self.d[0, 8] <= EPSILON or self.d[3, 8] <= EPSILON)
 
     def check_face_013_of_tetrahedron(self):
-        return not (self.d[0, 11] <= 0.0 or self.d[1, 11] <= 0.0 or self.d[3, 11] <= 0.0)
+        return not (self.d[0, 11] <= EPSILON or self.d[1, 11] <= EPSILON or self.d[3, 11] <= EPSILON)
 
     def check_face_023_of_tetrahedron(self):
-        return not (self.d[0, 12] <= 0.0 or self.d[2, 12] <= 0.0 or self.d[3, 12] <= 0.0)
+        return not (self.d[0, 12] <= EPSILON or self.d[2, 12] <= EPSILON or self.d[3, 12] <= EPSILON)
 
     def check_line_segment_13_of_tetrahedron(self):
-        return not (self.d[1, 9] <= 0.0 or self.d[3, 9] <= 0.0)
+        return not (self.d[1, 9] <= EPSILON or self.d[3, 9] <= EPSILON)
 
     def check_line_segment_23_of_tetrahedron(self):
-        return not (self.d[2, 10] <= 0.0 or self.d[3, 10] <= 0.0)
+        return not (self.d[2, 10] <= EPSILON or self.d[3, 10] <= EPSILON)
 
     def check_face_123_of_tetrahedron(self):
-        return not (self.d[1, 13] <= 0.0 or self.d[2, 13] <= 0.0 or self.d[3, 13] <= 0.0)
+        return not (self.d[1, 13] <= EPSILON or self.d[2, 13] <= EPSILON or self.d[3, 13] <= EPSILON)

Alternative contact models

Andrews et al.: SIGGRAPH'22 Course: Contact and Friction Simulation for Computer Graphics (2022). https://siggraphcontact.github.io/
Castro et al.: A Transition-Aware Method for the Simulation of Compliant Contact with Regularized Friction (2019). https://arxiv.org/pdf/1909.05700.pdf
Boos et al.: Volumetric Modeling and Experimental Validation of Normal Contact Dynamic Forces (2013). https://uwaterloo.ca/motion-research-group/sites/ca.motion-research-group/files/uploads/files/boos2013.pdf
Lembcke: Realtime Rigid Body Simulation Using Impulses (2006). http://files.slembcke.net/misc/RigidBodies.pdf
Hertz: Über die Berührung fester elastischer Körper (1881). https://home.uni-leipzig.de/pwm/web/download/Hertz1881.pdf
Hasegawa et al.: Real-Time Rigid Body Simulation Based on Volumetric Penalty Method (2003). https://www.researchgate.net/publication/221012120_Real-Time_Rigid_Body_Simulation_Based_on_Volumetric_Penalty_Method
Terzopoulos et al.: Elastically Deformable Models (1987). http://web.cs.ucla.edu/~dt/papers/siggraph87/siggraph87.pdf
Kaufman et al.: Staggered Projections for Frictional Contact in Multibody Systems (2008). http://www.cs.ubc.ca/labs/sensorimotor/projects/sp_sigasia08/KSJP08.pdf
Todorov: A convex, smooth and invertible contact model for trajectory optimization (2011). https://homes.cs.washington.edu/~todorov/papers/TodorovICRA11.pdf
Drumwright et al.: Modeling Contact Friction and Joint Friction in Dynamic Robotic Simulation Using the Principle of Maximum Dissipation (2010). https://cse-robotics.engr.tamu.edu/dshell/papers/wafr2010contact.pdf
MuJoCo documentation: https://mujoco.readthedocs.io/en/latest/computation.html#cocontact
Spring-damper contact model: https://www.sky-engin.jp/en/MBDynTutorial/chap26/chap26.html
Constraint rigid body simulation: https://www.toptal.com/game/video-game-physics-part-iii-constrained-rigid-body-simulation
Smith et al.: Stable Neo-Hookean Flesh Simulation (2017). https://graphics.pixar.com/library/StableElasticity/

Books

Ruina et al.: Introduction to Mechanics for Engineers (2019). http://ruina.tam.cornell.edu/Book/RuinaPratap-July-12-2019.pdf
Sueda: Analytically Differentiable Articulated Rigid Body Dynamics (2021). https://github.com/sueda/redmax/blob/master/notes.pdf
Kim et al.: Dynamic Deformables: Implementation and Production Practicalities (2020). https://graphics.pixar.com/library/DynamicDeformablesSiggraph2020/paper.pdf

Differentiable collision detection

GJK error

How to reproduce: run examples/visualizations/vis_robot_collision_objects.py without AABB tree

Release 0.5.0

Features

Use trimesh as a fallback solution to load meshes from URDF files when Open3D does not work
Collision margin collider #30
Containment tests #32

Performance

Faster inversion of transformation matrices with numba

Hydroelastic contact / pressure field model

Current State

Literature

Elandt et al.: A pressure field model for fast, robust approximation of net contact force and moment between nominally rigid objects (2018)
- PDF: https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=8968548
- preprint: https://www.researchgate.net/publication/332669476_A_pressure_field_model_for_fast_robust_approximation_of_net_contact_force_and_moment_between_nominally_rigid_objects
- Symbols
  - epsilon: potentials of vertices, closest distance to surface of the mesh
  - nu: vertex of a tetrahedron
  - zeta: barycentric coordinates of a point in terms of vertices of a tetrahedron; non-negative, sum to one
  - X: transforms zeta to Cartesian frame; columns are the vertices of the tetrahedron, last row is 1
Huang et al.: An Efficient Implementation of Pressure Field Models
- PDF: https://www.ekzhang.com/assets/pdf/Hydroelastics.pdf
- code: https://github.com/ekzhang/hydroelastics/
Masterjohn et al.: Velocity Level Approximation of Pressure Field Contact Patches (2022)
- blog: https://medium.com/toyotaresearch/rethinking-contact-simulation-for-robot-manipulation-434a56b5ec88
- PDF: https://arxiv.org/pdf/2110.04157.pdf
Drake
- user guide: https://drake.mit.edu/doxygen_cxx/group__hydroelastic__user__guide.html
- API documentation: https://drake.mit.edu/doxygen_cxx/classdrake_1_1geometry_1_1_contact_surface.html
- video: https://www.youtube.com/watch?v=HL1-yIvwSHI&t=7s

TODO

Broad Phase Bug

We currently use the aabbtree library for broad phase collision detection. It seems to have a bug as it filters too many potentially overlapping AABBs.

I assume the problem might be in the function _unique_pairs of the library.

How to Reproduce

Uncomment broad phase collision detection in distance3d/hydroelastic_contact/_broad_phase.py.

Modify examples/visualization/vis_pressure_field.py to the following objects:

import pytransform3d.rotations as pr


rigid_body1 = hydroelastic_contact.RigidBody.make_sphere(0.13 * np.ones(3), 0.15, 2)
cube2origin = np.eye(4)
cube2origin[:3, :3] = pr.active_matrix_from_extrinsic_euler_zyx([0.1, 0.3, 0.5])
cube2origin[:3, 3] = 0.25 * np.ones(3)
rigid_body2 = hydroelastic_contact.RigidBody.make_cube(cube2origin, 0.15)

Generate GIF

ffmpeg -i Screencast*.mp4 -r 15 -vf scale=512:-1 -ss 00:00:00 -to 00:00:04 hydroelastic_pressure_field.gif
ffmpeg -i Screencast*.mp4 -r 15 -vf scale=512:-1 -filter:v "setpts=0.05*PTS" hydroelastic_pressure_field.gif

ffmpeg -i 'Bildschirmaufnahme 2022-09-02 17:41:36.mp4' img%04d.jpg
convert -resize 50% -delay 5 -loop 0 img*.jpg img.gif

Release 0.3.0

Features

Distance
- from line segment to circle
- from plane to plane
- from plane to triangle
- from plane to rectangle
- from plane to box
- from plane to ellipsoid

Performance Improvements

Significantly faster AABB computation for boxes, cylinders, and capsules
Significantly faster self collision detection by pruning
6x faster distance from point to box

Getting non reproducible collisions and penetration-depths

I have a huge set of meshes ( >5000 ) in my collision detection scene.
If I execute gjk.gjk on those pairs of meshes with overlapping AABBs, I get ~227000 collisions.
Filtering out the collisions that are of interest for my use-case, I still find false-positives with a penetration-depth of non-zero using epa.epa.
Those false-positive collisions (sometimes) seem to be related to two meshes that are barely touching, see example scene.

If I re-run the collision again in a test script ( using the same vertex data of the two supposedly colliding meshes ) I get a collision with different penetration-depth. The difference is sometimes "huge", e.g. first time I get "4.0" after re-testing it I get "0.0".
If they are indeed barely touching, I would expect a tiny penetration-depth in both cases.

Or if I shuffle the order of collision testing, I get different penetration-depth for the same pair of meshes.
I.e. the following pseudo code of my own usage of distance3d yields different penetration-depth results for same mesh pairs that are in-collision:

broad-phase, find potentially collision of meshes by overlapping AABBs of all meshes
- e.g. this yields pair mesh indices [ (1,2), (5,8), (23,44), (5,15) ]
find collisions using gjk.gjk and epa.epa between combination of AABB-overlapping-mesh-pair
- i.e. I test mesh 1 vs 2, 5 vs 8, 23 vs 44, 5 vs 15
shuffle list from 1.
- e.g. this yields pair mesh indices [ (5,8), (1,2), (5,15), (23,44) ]
redo 2. with the shuffled list of 3.
- i.e. I test mesh 5 vs 8, 1 vs 2, 5 vs 15, 23 vs 44

results from 2. and 4. sometimes differ for the returned in-collision-flag and penetration-depth for the same pair of meshes !

Just swapping the two colliders or repeating the collisions yields different results for penetration-depth too.

def narrow_collision(collider_a, collider_b):
    distance, closest_point_a, closest_point_b, simplex = gjk.gjk(
        collider_a, collider_b
    )
    colliding = numpy.allclose(distance, 0)
    penetration_depth = 0.0
    if colliding:
        minimum_translation_vector, minkowski_faces, success = epa.epa(
            simplex,
            collider_a,
            collider_b,
            max_faces=512,
        )
        if success and not numpy.allclose(minimum_translation_vector, 0):
            penetration_depth = numpy.linalg.norm(minimum_translation_vector)
    return colliding, penetration_depth

colliding_one_two, penetration_depth_one_two = narrow_collision(
    my_collider_one, my_collider_two
)
colliding_two_one, penetration_depth_two_one = narrow_collision(
    my_collider_two, my_collider_one
)
# penetration_depth_one_two != penetration_depth_two_one

Summary: I see non reproducible results when

using a different order of mesh-vs-mesh collisions
or just swapping the meshes for a single mesh-vs-mesh collision
or just repeating a single mesh-vs-mesh collision

e.g. examples of penetration-depths for repeated mesh-vs-mesh collisions I have seen ( the three cases are distinct pairs of meshes ):

swapping meshes only gives same result for the first attempt
a vs b : [7.29998779296875, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
b vs a : [7.29998779296875, 7.29998779296875, 7.29998779296875, 7.29998779296875, 7.29998779296875, 7.29998779296875, 7.29998779296875, 7.29998779296875, 7.29998779296875, 7.29998779296875]
swapping meshes always gives different results by value
c vs d : [6.688873871420793, 6.688873871420793, 6.688873871420793, 6.688873871420793, 6.688873871420793, 6.688873871420793, 6.688873871420793, 6.688873871420793, 6.688873871420793, 6.688873871420793]
d vs c : [3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0]
swapping meshes always gives different results by category ( penetrating vs not-penetrating )
e vs f : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
f vs e : [3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0]

Due to the vast amount of collisions I have to evaluate in my test case, it's hard to pin those collision result differences to just meshes that are barely touching.
I'll have to further investigate on a mesh-by-mesh case.

Using:

MacOS Ventura 13.4.1 (22F82) on M1 arm64
Python 3.10.6
distance3d from master branch commit - dede764
numba 0.57.0
numpy 1.24.3

Can't import every module

Hello,

I installed distance3d , which looks awesome by the way, from pypi but the only modules that I can import are containment, geometry, mesh, and utils. Python can't seem to import the other modules though.

Have I done something wrong? Sorry if I missed something

Thank you

alexanderfabisch / distance3d Goto Github PK

distance3d's People

Contributors

Stargazers

Watchers

Forkers

distance3d's Issues

Features

Contact Points in Contact Plane

Overlapping Volumes

Combined Contact Normal

Code

Features

Features

Features

Features

Bugfixes

Features

Performance

Refactoring

Features

Performance

Current State

Literature

TODO

Broad Phase Bug

Links

How to Reproduce

Generate GIF

Features

Performance Improvements

Recommend Projects

Recommend Topics

Recommend Org