Merge branch 'dev' into mm-fixes

.github/workflows/ccpp.yml

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

.gitignore

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

examples/python/build.sh

@@ -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`

examples/python/test.py

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

python/build.sh

@@ -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` 

src/openGJK.c

@@ -37,8 +37,8 @@
 #define mexPrintf  printf
 #endif
 
-#define eps_rel22  1e-5
+#define eps_rel22  1e-10
-#define eps_tot22  1e-14
+#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);
+    exeedtol_rel = (norm2(v) - dotProduct(v, w));
-    if (exeedtol_rel) {
+    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) {