Merge branch 'dev' into mm-fixes

.github/workflows/ccpp.yml

@@ -12,8 +12,8 @@ jobs:
     - 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
+      run: export CPATH=/usr/include/eigen3 && cd examples/python && source build.sh
     - name: Test
       run: |
-        cd python
+        cd examples/python
         python3 -m pytest -s -v test.py

.gitignore

@@ -210,3 +210,8 @@ callgrind.out.*
 # Jetbrains IDE project files
 /.idea
 /*.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

@@ -2,7 +2,10 @@ 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):
     """
@@ -29,7 +32,7 @@ def test_line_point_distance(delta):
     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))
+    assert(np.isclose(distance, actual_distance, atol=settol() ))
 
 
 @pytest.mark.parametrize("delta", [0.1, 1e-12, 0])
@@ -43,7 +46,7 @@ def test_line_line_distance(delta):
     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))
+    assert(np.isclose(distance, actual_distance, atol=settol() ))
 
 
 @pytest.mark.parametrize("delta", [0.1**(3*i) for i in range(6)])
@@ -58,7 +61,8 @@ def test_tri_distance(delta):
     distance = opengjk.gjk(tri_1, tri_2) 
     print("Computed distance ", distance, "Actual distance ", actual_distance)
 
-    assert(np.isclose(distance, actual_distance, atol=1e-15))
+    #embed()
+    assert(np.isclose(distance, actual_distance, atol=settol() ))
 
 
 @pytest.mark.parametrize("delta", [0.1*0.1**(3*i) for i in range(6)])
@@ -74,7 +78,7 @@ def test_quad_distance2d(delta):
     distance = opengjk.gjk(quad_1, quad_2)
     print("Computed distance ", distance, "Actual distance ", actual_distance)
 
-    assert(np.isclose(distance, actual_distance, atol=1e-15))
+    assert(np.isclose(distance, actual_distance, atol=settol() ))
 
 
 @pytest.mark.parametrize("delta", [1*0.5**(3*i) for i in range(7)])
@@ -88,7 +92,7 @@ def test_tetra_distance_3d(delta):
     distance = opengjk.gjk(tetra_1, tetra_2)
     print("Computed distance ", distance, "Actual distance ", actual_distance)
 
-    assert(np.isclose(distance, actual_distance, atol=1e-15))
+    assert(np.isclose(distance, actual_distance, atol=settol() ))
 
 
 @pytest.mark.parametrize("delta", [(-1)**i*np.sqrt(2)*0.1**(3*i)
@@ -103,11 +107,11 @@ def test_tetra_collision_3d(delta):
     distance = opengjk.gjk(tetra_1, tetra_2)
 
     if delta < 0:
-        assert(np.isclose(distance, 0, atol=1e-15))
+        assert(np.isclose(distance, 0, atol=settol()))
     else:
         print("Computed distance ", distance,
               "Actual distance ", actual_distance)
-        assert(np.isclose(distance, actual_distance, atol=1e-15))
+        assert(np.isclose(distance, actual_distance, atol=settol()))
 
 
 @pytest.mark.parametrize("delta", [0, -0.1, -0.49, -0.51])
@@ -133,11 +137,11 @@ def test_hex_collision_3d(delta):
     distance = opengjk.gjk(hex_1, hex_2)
 
     if P0[0] < 1:
-        assert(np.isclose(distance, 0, atol=1e-15))
+        assert(np.isclose(distance, 0, atol=settol()))
     else:
         print("Computed distance ", distance,
               "Actual distance ", actual_distance)
-        assert(np.isclose(distance, actual_distance, atol=1e-15))
+        assert(np.isclose(distance, actual_distance, atol=settol()))
 
 
 @pytest.mark.parametrize("c0", [0, 1, 2, 3])

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,8 +766,9 @@ 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));