turtlebasket
/
openGJK
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'dev' into mm-fixes

fixes-turtlebasket
Mattia Montanari 2020-05-21 17:52:28 +01:00 committed by GitHub
parent 07f444a0f1 0bb555f0a1
commit d090fafe02
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 222 additions and 212 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
.github/workflows/ccpp.yml vendored
View File

@ -1,19 +1,19 @@
name: C/C++ CI
on: [push]
jobs:
build:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- name: Install Eigen and Pybind11
run: sudo apt update && sudo apt install -y libeigen3-dev && pip3 install pybind11 pytest numpy scipy
- name: Compile
run: export CPATH=/usr/include/eigen3 && cd python && source build.sh
- name: Test
run: |
cd python
python3 -m pytest -s -v test.py
name: C/C++ CI
on: [push]
jobs:
build:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- name: Install Eigen and Pybind11
run: sudo apt update && sudo apt install -y libeigen3-dev && pip3 install pybind11 pytest numpy scipy
- name: Compile
run: export CPATH=/usr/include/eigen3 && cd examples/python && source build.sh
- name: Test
run: |
cd examples/python
python3 -m pytest -s -v test.py

7
.gitignore vendored
View File

@ -209,4 +209,9 @@ callgrind.out.*
# Jetbrains IDE project files
/.idea
/*.iml
/*.iml
# Pytest
.cache
.pytest_cache
.mypy_cache

2
examples/python/build.sh Executable file
View File

@ -0,0 +1,2 @@
#!/bin/bash
g++ -Wall -fPIC -fopenmp -shared `python3 -m pybind11 --includes` -I ../../include -I/usr/include/eigen3 pyopenGJK.cpp ../../src/openGJK.c -o opengjkc`python3-config --extension-suffix`

0
python/pyopenGJK.cpp → examples/python/pyopenGJK.cpp
View File

366
python/test.py → examples/python/test.py
View File

@ -1,181 +1,185 @@
import opengjkc as opengjk
from scipy.spatial.transform import Rotation as R
import numpy as np
import pytest
def distance_point_to_line_3D(P1, P2, point):
"""
distance from point to line
"""
return np.linalg.norm(np.cross(P2-P1, P1-point))/np.linalg.norm(P2-P1)
def distance_point_to_plane_3D(P1, P2, P3, point):
"""
Distance from point to plane
"""
return np.abs(np.dot(np.cross(P2-P1, P3-P1) /
np.linalg.norm(np.cross(P2-P1, P3-P1)), point-P2))
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0, -2])
def test_line_point_distance(delta):
line = np.array([[0.1, 0.2, 0.3], [0.5, 0.8, 0.7]], dtype=np.float64)
point_on_line = line[0] + 0.27*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
distance = opengjk.gjk(line, point)
actual_distance = distance_point_to_line_3D(
line[0], line[1], point)
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=1e-15))
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0])
def test_line_line_distance(delta):
line = np.array([[-0.5, -0.7, -0.3], [1, 2, 3]], dtype=np.float64)
point_on_line = line[0] + 0.38*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
line_2 = np.array([point, [2, 5, 6]], dtype=np.float64)
distance = opengjk.gjk(line, line_2)
actual_distance = distance_point_to_line_3D(
line[0], line[1], line_2[0])
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=1e-15))
@pytest.mark.parametrize("delta", [0.1**(3*i) for i in range(6)])
def test_tri_distance(delta):
tri_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
tri_2 = np.array([[1, delta, 0], [3, 1.2, 0], [
1, 1, 0]], dtype=np.float64)
P1 = tri_1[2]
P2 = tri_1[1]
point = tri_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.gjk(tri_1, tri_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=1e-15))
@pytest.mark.parametrize("delta", [0.1*0.1**(3*i) for i in range(6)])
def test_quad_distance2d(delta):
quad_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[1, 1, 0]], dtype=np.float64)
quad_2 = np.array([[0, 1+delta, 0], [2, 2, 0],
[2, 4, 0], [4, 4, 0]], dtype=np.float64)
P1 = quad_1[2]
P2 = quad_1[3]
point = quad_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.gjk(quad_1, quad_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=1e-15))
@pytest.mark.parametrize("delta", [1*0.5**(3*i) for i in range(7)])
def test_tetra_distance_3d(delta):
tetra_1 = np.array([[0, 0, 0.2], [1, 0, 0.1], [0, 1, 0.3],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.gjk(tetra_1, tetra_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=1e-15))
@pytest.mark.parametrize("delta", [(-1)**i*np.sqrt(2)*0.1**(3*i)
for i in range(6)])
def test_tetra_collision_3d(delta):
tetra_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.gjk(tetra_1, tetra_2)
if delta < 0:
assert(np.isclose(distance, 0, atol=1e-15))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=1e-15))
@pytest.mark.parametrize("delta", [0, -0.1, -0.49, -0.51])
def test_hex_collision_3d(delta):
hex_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0],
[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]],
dtype=np.float64)
P0 = np.array([1.5+delta, 1.5+delta, 0.5], dtype=np.float64)
P1 = np.array([2, 2, 1], dtype=np.float64)
P2 = np.array([2, 1.25, 0.25], dtype=np.float64)
P3 = P1 + P2 - P0
quad_1 = np.array([P0, P1, P2, P3], dtype=np.float64)
n = (np.cross(quad_1[1]-quad_1[0], quad_1[2]-quad_1[0]) /
np.linalg.norm(
np.cross(quad_1[1]-quad_1[0],
quad_1[2]-quad_1[0])))
quad_2 = quad_1 + n
hex_2 = np.zeros((8, 3), dtype=np.float64)
hex_2[:4, :] = quad_1
hex_2[4:, :] = quad_2
actual_distance = np.linalg.norm(
np.array([1, 1, P0[2]], dtype=np.float64)-hex_2[0])
distance = opengjk.gjk(hex_1, hex_2)
if P0[0] < 1:
assert(np.isclose(distance, 0, atol=1e-15))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=1e-15))
@pytest.mark.parametrize("c0", [0, 1, 2, 3])
@pytest.mark.parametrize("c1", [0, 1, 2, 3])
def test_cube_distance(c0, c1):
cubes = [np.array([[-1, -1, -1], [1, -1, -1], [-1, 1, -1], [1, 1, -1],
[-1, -1, 1], [1, -1, 1], [-1, 1, 1], [1, 1, 1]],
dtype=np.float64)]
r = R.from_euler('z', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
r = R.from_euler('y', np.arctan2(1.0, np.sqrt(2)))
cubes.append(r.apply(cubes[1]))
r = R.from_euler('y', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
dx = cubes[c0][:,0].max() - cubes[c1][:,0].min()
cube0 = cubes[c0]
for delta in [1e8, 1.0, 1e-4, 1e-8, 1e-12]:
cube1 = cubes[c1] + np.array([dx + delta, 0, 0])
distance = opengjk.gjk(cube0, cube1)
print(distance, delta)
assert(np.isclose(distance, delta))
def test_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = np.random.rand(i, 3)
arr2 = np.random.rand(j, 3)
opengjk.gjk(arr1, arr2)
def test_large_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = 10000.0*np.random.rand(i, 3)
arr2 = 10000.0*np.random.rand(j, 3)
opengjk.gjk(arr1, arr2)
import opengjkc as opengjk
from scipy.spatial.transform import Rotation as R
import numpy as np
import pytest
#from IPython import embed
def settol():
return 1e-12
def distance_point_to_line_3D(P1, P2, point):
"""
distance from point to line
"""
return np.linalg.norm(np.cross(P2-P1, P1-point))/np.linalg.norm(P2-P1)
def distance_point_to_plane_3D(P1, P2, P3, point):
"""
Distance from point to plane
"""
return np.abs(np.dot(np.cross(P2-P1, P3-P1) /
np.linalg.norm(np.cross(P2-P1, P3-P1)), point-P2))
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0, -2])
def test_line_point_distance(delta):
line = np.array([[0.1, 0.2, 0.3], [0.5, 0.8, 0.7]], dtype=np.float64)
point_on_line = line[0] + 0.27*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
distance = opengjk.gjk(line, point)
actual_distance = distance_point_to_line_3D(
line[0], line[1], point)
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0])
def test_line_line_distance(delta):
line = np.array([[-0.5, -0.7, -0.3], [1, 2, 3]], dtype=np.float64)
point_on_line = line[0] + 0.38*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
line_2 = np.array([point, [2, 5, 6]], dtype=np.float64)
distance = opengjk.gjk(line, line_2)
actual_distance = distance_point_to_line_3D(
line[0], line[1], line_2[0])
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1**(3*i) for i in range(6)])
def test_tri_distance(delta):
tri_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
tri_2 = np.array([[1, delta, 0], [3, 1.2, 0], [
1, 1, 0]], dtype=np.float64)
P1 = tri_1[2]
P2 = tri_1[1]
point = tri_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.gjk(tri_1, tri_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
#embed()
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1*0.1**(3*i) for i in range(6)])
def test_quad_distance2d(delta):
quad_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[1, 1, 0]], dtype=np.float64)
quad_2 = np.array([[0, 1+delta, 0], [2, 2, 0],
[2, 4, 0], [4, 4, 0]], dtype=np.float64)
P1 = quad_1[2]
P2 = quad_1[3]
point = quad_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.gjk(quad_1, quad_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [1*0.5**(3*i) for i in range(7)])
def test_tetra_distance_3d(delta):
tetra_1 = np.array([[0, 0, 0.2], [1, 0, 0.1], [0, 1, 0.3],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.gjk(tetra_1, tetra_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [(-1)**i*np.sqrt(2)*0.1**(3*i)
for i in range(6)])
def test_tetra_collision_3d(delta):
tetra_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.gjk(tetra_1, tetra_2)
if delta < 0:
assert(np.isclose(distance, 0, atol=settol()))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol()))
@pytest.mark.parametrize("delta", [0, -0.1, -0.49, -0.51])
def test_hex_collision_3d(delta):
hex_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0],
[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]],
dtype=np.float64)
P0 = np.array([1.5+delta, 1.5+delta, 0.5], dtype=np.float64)
P1 = np.array([2, 2, 1], dtype=np.float64)
P2 = np.array([2, 1.25, 0.25], dtype=np.float64)
P3 = P1 + P2 - P0
quad_1 = np.array([P0, P1, P2, P3], dtype=np.float64)
n = (np.cross(quad_1[1]-quad_1[0], quad_1[2]-quad_1[0]) /
np.linalg.norm(
np.cross(quad_1[1]-quad_1[0],
quad_1[2]-quad_1[0])))
quad_2 = quad_1 + n
hex_2 = np.zeros((8, 3), dtype=np.float64)
hex_2[:4, :] = quad_1
hex_2[4:, :] = quad_2
actual_distance = np.linalg.norm(
np.array([1, 1, P0[2]], dtype=np.float64)-hex_2[0])
distance = opengjk.gjk(hex_1, hex_2)
if P0[0] < 1:
assert(np.isclose(distance, 0, atol=settol()))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol()))
@pytest.mark.parametrize("c0", [0, 1, 2, 3])
@pytest.mark.parametrize("c1", [0, 1, 2, 3])
def test_cube_distance(c0, c1):
cubes = [np.array([[-1, -1, -1], [1, -1, -1], [-1, 1, -1], [1, 1, -1],
[-1, -1, 1], [1, -1, 1], [-1, 1, 1], [1, 1, 1]],
dtype=np.float64)]
r = R.from_euler('z', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
r = R.from_euler('y', np.arctan2(1.0, np.sqrt(2)))
cubes.append(r.apply(cubes[1]))
r = R.from_euler('y', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
dx = cubes[c0][:,0].max() - cubes[c1][:,0].min()
cube0 = cubes[c0]
for delta in [1e8, 1.0, 1e-4, 1e-8, 1e-12]:
cube1 = cubes[c1] + np.array([dx + delta, 0, 0])
distance = opengjk.gjk(cube0, cube1)
print(distance, delta)
assert(np.isclose(distance, delta))
def test_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = np.random.rand(i, 3)
arr2 = np.random.rand(j, 3)
opengjk.gjk(arr1, arr2)
def test_large_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = 10000.0*np.random.rand(i, 3)
arr2 = 10000.0*np.random.rand(j, 3)
opengjk.gjk(arr1, arr2)

2
python/build.sh
View File

@ -1,2 +0,0 @@
#!/bin/bash
g++ -Wall -fPIC -fopenmp -shared `python3 -m pybind11 --includes` -I../include -I/usr/include/eigen3 pyopenGJK.cpp ../src/openGJK.c -o opengjkc`python3-config --extension-suffix`

19
src/openGJK.c
View File

@ -37,8 +37,8 @@
#define mexPrintf printf
#endif
#define eps_rel22 1e-5
#define eps_tot22 1e-14
#define eps_rel22 1e-10
#define eps_tot22 1e-12
/* Select distance sub-algorithm */
@ -295,7 +295,7 @@ inline static void S2D(struct simplex * s, double *v)
}
else if (hff1f_s13) {
if (hff2f_32) {
projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c
projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update v
return; // Return V{1,2,3}
}
else
@ -675,7 +675,7 @@ double gjk(struct bd bd1, struct bd bd2, struct simplex *s) {
int k = 0; /**< Iteration counter */
int i; /**< General purpose counter */
int mk = 5000; /**< Maximum number of iterations of the GJK algorithm */
int mk = 25; /**< Maximum number of iterations of the GJK algorithm */
int absTestin;
double norm2Wmax = 0;
double tesnorm;
@ -685,7 +685,7 @@ double gjk(struct bd bd1, struct bd bd2, struct simplex *s) {
double eps_rel = eps_rel22; /**< Tolerance on relative */
double eps_rel2 = eps_rel * eps_rel;
double eps_tot = eps_tot22;
int exeedtol_rel = 0; /**< Flag for 1st exit condition */
double exeedtol_rel; /**< Test for 1st exit condition */
int nullV = 0;
#ifdef DEBUG
@ -738,8 +738,8 @@ double gjk(struct bd bd1, struct bd bd2, struct simplex *s) {
w[t] = bd1.s[t] - bd2.s[t];
/* Test first exit condition (new point already in simplex/can't move further) */
exeedtol_rel = (norm2(v) - dotProduct(v, w)) <= eps_rel2 * norm2(v);
if (exeedtol_rel) {
exeedtol_rel = (norm2(v) - dotProduct(v, w));
if ( exeedtol_rel <= (eps_rel * norm2(v)) || exeedtol_rel < eps_tot22) {
break;
}
@ -766,9 +766,10 @@ double gjk(struct bd bd1, struct bd bd2, struct simplex *s) {
}
absTestin = (norm2(v) <= (eps_tot * eps_tot * norm2Wmax));
if (absTestin)
if (absTestin) {
break;
}
} while ((s->nvrtx != 4) && (k != mk));
if (k == mk) {