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

Cython wrapper trial 5 malloc correct

fixes-turtlebasket
Daniel Song 2020-05-21 12:30:33 +01:00
parent fe821367e3
commit d5dc76021f
4 changed files with 237 additions and 239 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Cython/__pycache__/test.cpython-37-pytest-5.4.2.pyc
View File

Binary file not shown.

207
Cython/openGJK_cython.c
View File

@ -1784,7 +1784,6 @@ static const char __pyx_k_step[] = "step";
static const char __pyx_k_stop[] = "stop"; static const char __pyx_k_stop[] = "stop";
static const char __pyx_k_test[] = "__test__"; static const char __pyx_k_test[] = "__test__";
static const char __pyx_k_ASCII[] = "ASCII"; static const char __pyx_k_ASCII[] = "ASCII";
static const char __pyx_k_bdptr[] = "bdptr";
static const char __pyx_k_class[] = "__class__"; static const char __pyx_k_class[] = "__class__";
static const char __pyx_k_error[] = "error"; static const char __pyx_k_error[] = "error";
static const char __pyx_k_flags[] = "flags"; static const char __pyx_k_flags[] = "flags";
@ -1899,7 +1898,6 @@ static PyObject *__pyx_n_s_axis;
static PyObject *__pyx_n_s_base; static PyObject *__pyx_n_s_base;
static PyObject *__pyx_n_s_bd1; static PyObject *__pyx_n_s_bd1;
static PyObject *__pyx_n_s_bd2; static PyObject *__pyx_n_s_bd2;
static PyObject *__pyx_n_s_bdptr;
static PyObject *__pyx_n_s_bod1; static PyObject *__pyx_n_s_bod1;
static PyObject *__pyx_n_s_bod2; static PyObject *__pyx_n_s_bod2;
static PyObject *__pyx_n_s_c; static PyObject *__pyx_n_s_c;
@ -2137,7 +2135,6 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
int __pyx_v_i; int __pyx_v_i;
int __pyx_v_j; int __pyx_v_j;
double __pyx_v_answer; double __pyx_v_answer;
struct bd *__pyx_v_bdptr;
__Pyx_memviewslice __pyx_v_narr1 = { 0, 0, { 0 }, { 0 }, { 0 } }; __Pyx_memviewslice __pyx_v_narr1 = { 0, 0, { 0 }, { 0 }, { 0 } };
__Pyx_memviewslice __pyx_v_narr2 = { 0, 0, { 0 }, { 0 }, { 0 } }; __Pyx_memviewslice __pyx_v_narr2 = { 0, 0, { 0 }, { 0 }, { 0 } };
PyObject *__pyx_r = NULL; PyObject *__pyx_r = NULL;
@ -2609,107 +2606,57 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
/* "openGJK_cython.pyx":55 /* "openGJK_cython.pyx":55
* *
* # Allocate memory for pointer (not working) * # Allocate memory for pointer (not working)
* cdef bd *bdptr = <bd *> malloc(sizeof(bd)) # <<<<<<<<<<<<<< * bd1.coord = <double **> malloc(bd1.numpoints * sizeof(double *)) # <<<<<<<<<<<<<<
* bd2.coord = <double **> malloc(bd2.numpoints * sizeof(double *))
* *
* if bd1.numpoints >= bd2.numpoints:
*/ */
__pyx_v_bdptr = ((struct bd *)malloc((sizeof(struct bd)))); __pyx_v_bd1.coord = ((double **)malloc((__pyx_v_bd1.numpoints * (sizeof(double *)))));
/* "openGJK_cython.pyx":57 /* "openGJK_cython.pyx":56
* cdef bd *bdptr = <bd *> malloc(sizeof(bd)) * # Allocate memory for pointer (not working)
* bd1.coord = <double **> malloc(bd1.numpoints * sizeof(double *))
* bd2.coord = <double **> malloc(bd2.numpoints * sizeof(double *)) # <<<<<<<<<<<<<<
* *
* if bd1.numpoints >= bd2.numpoints: # <<<<<<<<<<<<<<
* bdptr.coord[0] = <double *> malloc(3 * sizeof(double))
* bdptr.coord = <double **> malloc(bd1.numpoints * sizeof(bdptr.coord[0]))
*/
__pyx_t_3 = ((__pyx_v_bd1.numpoints >= __pyx_v_bd2.numpoints) != 0);
if (__pyx_t_3) {
/* "openGJK_cython.pyx":58
*
* if bd1.numpoints >= bd2.numpoints:
* bdptr.coord[0] = <double *> malloc(3 * sizeof(double)) # <<<<<<<<<<<<<<
* bdptr.coord = <double **> malloc(bd1.numpoints * sizeof(bdptr.coord[0]))
* *
*/ */
(__pyx_v_bdptr->coord[0]) = ((double *)malloc((3 * (sizeof(double))))); __pyx_v_bd2.coord = ((double **)malloc((__pyx_v_bd2.numpoints * (sizeof(double *)))));
/* "openGJK_cython.pyx":59 /* "openGJK_cython.pyx":59
* if bd1.numpoints >= bd2.numpoints:
* bdptr.coord[0] = <double *> malloc(3 * sizeof(double))
* bdptr.coord = <double **> malloc(bd1.numpoints * sizeof(bdptr.coord[0])) # <<<<<<<<<<<<<<
*
* else:
*/
__pyx_v_bdptr->coord = ((double **)malloc((__pyx_v_bd1.numpoints * (sizeof((__pyx_v_bdptr->coord[0]))))));
/* "openGJK_cython.pyx":57
* cdef bd *bdptr = <bd *> malloc(sizeof(bd))
*
* if bd1.numpoints >= bd2.numpoints: # <<<<<<<<<<<<<<
* bdptr.coord[0] = <double *> malloc(3 * sizeof(double))
* bdptr.coord = <double **> malloc(bd1.numpoints * sizeof(bdptr.coord[0]))
*/
goto __pyx_L5;
}
/* "openGJK_cython.pyx":62
*
* else:
* bdptr.coord[0] = <double *> malloc(3 * sizeof(double)) # <<<<<<<<<<<<<<
* bdptr.coord = <double **> malloc(bd2.numpoints * sizeof(bdptr.coord[0]))
*
*/
/*else*/ {
(__pyx_v_bdptr->coord[0]) = ((double *)malloc((3 * (sizeof(double)))));
/* "openGJK_cython.pyx":63
* else:
* bdptr.coord[0] = <double *> malloc(3 * sizeof(double))
* bdptr.coord = <double **> malloc(bd2.numpoints * sizeof(bdptr.coord[0])) # <<<<<<<<<<<<<<
*
*
*/
__pyx_v_bdptr->coord = ((double **)malloc((__pyx_v_bd2.numpoints * (sizeof((__pyx_v_bdptr->coord[0]))))));
}
__pyx_L5:;
/* "openGJK_cython.pyx":66
* *
* *
* print("Break 2")#-------------------------------------------------- # <<<<<<<<<<<<<< * print("Break 2")#-------------------------------------------------- # <<<<<<<<<<<<<<
* *
* # Create numpy-array MemoryView * # Create numpy-array MemoryView
*/ */
__pyx_t_1 = __Pyx_PyObject_Call(__pyx_builtin_print, __pyx_tuple__2, NULL); if (unlikely(!__pyx_t_1)) __PYX_ERR(0, 66, __pyx_L1_error) __pyx_t_1 = __Pyx_PyObject_Call(__pyx_builtin_print, __pyx_tuple__2, NULL); if (unlikely(!__pyx_t_1)) __PYX_ERR(0, 59, __pyx_L1_error)
__Pyx_GOTREF(__pyx_t_1); __Pyx_GOTREF(__pyx_t_1);
__Pyx_DECREF(__pyx_t_1); __pyx_t_1 = 0; __Pyx_DECREF(__pyx_t_1); __pyx_t_1 = 0;
/* "openGJK_cython.pyx":70 /* "openGJK_cython.pyx":63
* # Create numpy-array MemoryView * # Create numpy-array MemoryView
* cdef: * cdef:
* double [:,:] narr1 = bod1 # <<<<<<<<<<<<<< * double [:,:] narr1 = bod1 # <<<<<<<<<<<<<<
* double [:,:] narr2 = bod2 * double [:,:] narr2 = bod2
* *
*/ */
__pyx_t_7 = __Pyx_PyObject_to_MemoryviewSlice_dsds_double(__pyx_v_bod1, PyBUF_WRITABLE); if (unlikely(!__pyx_t_7.memview)) __PYX_ERR(0, 70, __pyx_L1_error) __pyx_t_7 = __Pyx_PyObject_to_MemoryviewSlice_dsds_double(__pyx_v_bod1, PyBUF_WRITABLE); if (unlikely(!__pyx_t_7.memview)) __PYX_ERR(0, 63, __pyx_L1_error)
__pyx_v_narr1 = __pyx_t_7; __pyx_v_narr1 = __pyx_t_7;
__pyx_t_7.memview = NULL; __pyx_t_7.memview = NULL;
__pyx_t_7.data = NULL; __pyx_t_7.data = NULL;
/* "openGJK_cython.pyx":71 /* "openGJK_cython.pyx":64
* cdef: * cdef:
* double [:,:] narr1 = bod1 * double [:,:] narr1 = bod1
* double [:,:] narr2 = bod2 # <<<<<<<<<<<<<< * double [:,:] narr2 = bod2 # <<<<<<<<<<<<<<
* *
* print(narr2[0,0]) # output a <double>, works fine * print(narr2[0,0]) # output a <double>, works fine
*/ */
__pyx_t_7 = __Pyx_PyObject_to_MemoryviewSlice_dsds_double(__pyx_v_bod2, PyBUF_WRITABLE); if (unlikely(!__pyx_t_7.memview)) __PYX_ERR(0, 71, __pyx_L1_error) __pyx_t_7 = __Pyx_PyObject_to_MemoryviewSlice_dsds_double(__pyx_v_bod2, PyBUF_WRITABLE); if (unlikely(!__pyx_t_7.memview)) __PYX_ERR(0, 64, __pyx_L1_error)
__pyx_v_narr2 = __pyx_t_7; __pyx_v_narr2 = __pyx_t_7;
__pyx_t_7.memview = NULL; __pyx_t_7.memview = NULL;
__pyx_t_7.data = NULL; __pyx_t_7.data = NULL;
/* "openGJK_cython.pyx":73 /* "openGJK_cython.pyx":66
* double [:,:] narr2 = bod2 * double [:,:] narr2 = bod2
* *
* print(narr2[0,0]) # output a <double>, works fine # <<<<<<<<<<<<<< * print(narr2[0,0]) # output a <double>, works fine # <<<<<<<<<<<<<<
@ -2720,39 +2667,48 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
__pyx_t_9 = 0; __pyx_t_9 = 0;
if (__pyx_t_8 < 0) __pyx_t_8 += __pyx_v_narr2.shape[0]; if (__pyx_t_8 < 0) __pyx_t_8 += __pyx_v_narr2.shape[0];
if (__pyx_t_9 < 0) __pyx_t_9 += __pyx_v_narr2.shape[1]; if (__pyx_t_9 < 0) __pyx_t_9 += __pyx_v_narr2.shape[1];
__pyx_t_1 = PyFloat_FromDouble((*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr2.data + __pyx_t_8 * __pyx_v_narr2.strides[0]) ) + __pyx_t_9 * __pyx_v_narr2.strides[1]) )))); if (unlikely(!__pyx_t_1)) __PYX_ERR(0, 73, __pyx_L1_error) __pyx_t_1 = PyFloat_FromDouble((*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr2.data + __pyx_t_8 * __pyx_v_narr2.strides[0]) ) + __pyx_t_9 * __pyx_v_narr2.strides[1]) )))); if (unlikely(!__pyx_t_1)) __PYX_ERR(0, 66, __pyx_L1_error)
__Pyx_GOTREF(__pyx_t_1); __Pyx_GOTREF(__pyx_t_1);
__pyx_t_5 = __Pyx_PyObject_CallOneArg(__pyx_builtin_print, __pyx_t_1); if (unlikely(!__pyx_t_5)) __PYX_ERR(0, 73, __pyx_L1_error) __pyx_t_5 = __Pyx_PyObject_CallOneArg(__pyx_builtin_print, __pyx_t_1); if (unlikely(!__pyx_t_5)) __PYX_ERR(0, 66, __pyx_L1_error)
__Pyx_GOTREF(__pyx_t_5); __Pyx_GOTREF(__pyx_t_5);
__Pyx_DECREF(__pyx_t_1); __pyx_t_1 = 0; __Pyx_DECREF(__pyx_t_1); __pyx_t_1 = 0;
__Pyx_DECREF(__pyx_t_5); __pyx_t_5 = 0; __Pyx_DECREF(__pyx_t_5); __pyx_t_5 = 0;
/* "openGJK_cython.pyx":75 /* "openGJK_cython.pyx":68
* print(narr2[0,0]) # output a <double>, works fine * print(narr2[0,0]) # output a <double>, works fine
* *
* print("Break 3")#-------------------------------------------------- # <<<<<<<<<<<<<< * print("Break 3")#-------------------------------------------------- # <<<<<<<<<<<<<<
* *
* # Assign coordinate values (Segmentation Fault Here!!, ) * # Assign coordinate values (Segmentation Fault Here!!, )
*/ */
__pyx_t_5 = __Pyx_PyObject_Call(__pyx_builtin_print, __pyx_tuple__3, NULL); if (unlikely(!__pyx_t_5)) __PYX_ERR(0, 75, __pyx_L1_error) __pyx_t_5 = __Pyx_PyObject_Call(__pyx_builtin_print, __pyx_tuple__3, NULL); if (unlikely(!__pyx_t_5)) __PYX_ERR(0, 68, __pyx_L1_error)
__Pyx_GOTREF(__pyx_t_5); __Pyx_GOTREF(__pyx_t_5);
__Pyx_DECREF(__pyx_t_5); __pyx_t_5 = 0; __Pyx_DECREF(__pyx_t_5); __pyx_t_5 = 0;
/* "openGJK_cython.pyx":78 /* "openGJK_cython.pyx":71
* *
* # Assign coordinate values (Segmentation Fault Here!!, ) * # Assign coordinate values (Segmentation Fault Here!!, )
* for i in range(0, bd1.numpoints): # <<<<<<<<<<<<<< * for i in range(0, bd1.numpoints): # <<<<<<<<<<<<<<
* bd1.coord[i] = <double *> malloc(3 * sizeof(double))
* bd1.coord[i][0] = narr1[i,0] * bd1.coord[i][0] = narr1[i,0]
* bd1.coord[i][1] = narr1[i,1]
*/ */
__pyx_t_6 = __pyx_v_bd1.numpoints; __pyx_t_6 = __pyx_v_bd1.numpoints;
__pyx_t_10 = __pyx_t_6; __pyx_t_10 = __pyx_t_6;
for (__pyx_t_11 = 0; __pyx_t_11 < __pyx_t_10; __pyx_t_11+=1) { for (__pyx_t_11 = 0; __pyx_t_11 < __pyx_t_10; __pyx_t_11+=1) {
__pyx_v_i = __pyx_t_11; __pyx_v_i = __pyx_t_11;
/* "openGJK_cython.pyx":79 /* "openGJK_cython.pyx":72
* # Assign coordinate values (Segmentation Fault Here!!, ) * # Assign coordinate values (Segmentation Fault Here!!, )
* for i in range(0, bd1.numpoints): * for i in range(0, bd1.numpoints):
* bd1.coord[i] = <double *> malloc(3 * sizeof(double)) # <<<<<<<<<<<<<<
* bd1.coord[i][0] = narr1[i,0]
* bd1.coord[i][1] = narr1[i,1]
*/
(__pyx_v_bd1.coord[__pyx_v_i]) = ((double *)malloc((3 * (sizeof(double)))));
/* "openGJK_cython.pyx":73
* for i in range(0, bd1.numpoints):
* bd1.coord[i] = <double *> malloc(3 * sizeof(double))
* bd1.coord[i][0] = narr1[i,0] # <<<<<<<<<<<<<< * bd1.coord[i][0] = narr1[i,0] # <<<<<<<<<<<<<<
* bd1.coord[i][1] = narr1[i,1] * bd1.coord[i][1] = narr1[i,1]
* bd1.coord[i][2] = narr1[i,2] * bd1.coord[i][2] = narr1[i,2]
@ -2763,8 +2719,8 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
if (__pyx_t_8 < 0) __pyx_t_8 += __pyx_v_narr1.shape[1]; if (__pyx_t_8 < 0) __pyx_t_8 += __pyx_v_narr1.shape[1];
((__pyx_v_bd1.coord[__pyx_v_i])[0]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr1.data + __pyx_t_9 * __pyx_v_narr1.strides[0]) ) + __pyx_t_8 * __pyx_v_narr1.strides[1]) ))); ((__pyx_v_bd1.coord[__pyx_v_i])[0]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr1.data + __pyx_t_9 * __pyx_v_narr1.strides[0]) ) + __pyx_t_8 * __pyx_v_narr1.strides[1]) )));
/* "openGJK_cython.pyx":80 /* "openGJK_cython.pyx":74
* for i in range(0, bd1.numpoints): * bd1.coord[i] = <double *> malloc(3 * sizeof(double))
* bd1.coord[i][0] = narr1[i,0] * bd1.coord[i][0] = narr1[i,0]
* bd1.coord[i][1] = narr1[i,1] # <<<<<<<<<<<<<< * bd1.coord[i][1] = narr1[i,1] # <<<<<<<<<<<<<<
* bd1.coord[i][2] = narr1[i,2] * bd1.coord[i][2] = narr1[i,2]
@ -2776,7 +2732,7 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
if (__pyx_t_9 < 0) __pyx_t_9 += __pyx_v_narr1.shape[1]; if (__pyx_t_9 < 0) __pyx_t_9 += __pyx_v_narr1.shape[1];
((__pyx_v_bd1.coord[__pyx_v_i])[1]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr1.data + __pyx_t_8 * __pyx_v_narr1.strides[0]) ) + __pyx_t_9 * __pyx_v_narr1.strides[1]) ))); ((__pyx_v_bd1.coord[__pyx_v_i])[1]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr1.data + __pyx_t_8 * __pyx_v_narr1.strides[0]) ) + __pyx_t_9 * __pyx_v_narr1.strides[1]) )));
/* "openGJK_cython.pyx":81 /* "openGJK_cython.pyx":75
* bd1.coord[i][0] = narr1[i,0] * bd1.coord[i][0] = narr1[i,0]
* bd1.coord[i][1] = narr1[i,1] * bd1.coord[i][1] = narr1[i,1]
* bd1.coord[i][2] = narr1[i,2] # <<<<<<<<<<<<<< * bd1.coord[i][2] = narr1[i,2] # <<<<<<<<<<<<<<
@ -2790,21 +2746,30 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
((__pyx_v_bd1.coord[__pyx_v_i])[2]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr1.data + __pyx_t_9 * __pyx_v_narr1.strides[0]) ) + __pyx_t_8 * __pyx_v_narr1.strides[1]) ))); ((__pyx_v_bd1.coord[__pyx_v_i])[2]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr1.data + __pyx_t_9 * __pyx_v_narr1.strides[0]) ) + __pyx_t_8 * __pyx_v_narr1.strides[1]) )));
} }
/* "openGJK_cython.pyx":84 /* "openGJK_cython.pyx":78
* *
* *
* for j in range(0, bd2.numpoints): # <<<<<<<<<<<<<< * for j in range(0, bd2.numpoints): # <<<<<<<<<<<<<<
* bd2.coord[j] = <double *> malloc(3 * sizeof(double))
* bd2.coord[j][0] = narr2[j,0] * bd2.coord[j][0] = narr2[j,0]
* bd2.coord[j][1] = narr2[j,1]
*/ */
__pyx_t_6 = __pyx_v_bd2.numpoints; __pyx_t_6 = __pyx_v_bd2.numpoints;
__pyx_t_10 = __pyx_t_6; __pyx_t_10 = __pyx_t_6;
for (__pyx_t_11 = 0; __pyx_t_11 < __pyx_t_10; __pyx_t_11+=1) { for (__pyx_t_11 = 0; __pyx_t_11 < __pyx_t_10; __pyx_t_11+=1) {
__pyx_v_j = __pyx_t_11; __pyx_v_j = __pyx_t_11;
/* "openGJK_cython.pyx":85 /* "openGJK_cython.pyx":79
* *
* for j in range(0, bd2.numpoints): * for j in range(0, bd2.numpoints):
* bd2.coord[j] = <double *> malloc(3 * sizeof(double)) # <<<<<<<<<<<<<<
* bd2.coord[j][0] = narr2[j,0]
* bd2.coord[j][1] = narr2[j,1]
*/
(__pyx_v_bd2.coord[__pyx_v_j]) = ((double *)malloc((3 * (sizeof(double)))));
/* "openGJK_cython.pyx":80
* for j in range(0, bd2.numpoints):
* bd2.coord[j] = <double *> malloc(3 * sizeof(double))
* bd2.coord[j][0] = narr2[j,0] # <<<<<<<<<<<<<< * bd2.coord[j][0] = narr2[j,0] # <<<<<<<<<<<<<<
* bd2.coord[j][1] = narr2[j,1] * bd2.coord[j][1] = narr2[j,1]
* bd2.coord[j][2] = narr2[j,2] * bd2.coord[j][2] = narr2[j,2]
@ -2815,8 +2780,8 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
if (__pyx_t_9 < 0) __pyx_t_9 += __pyx_v_narr2.shape[1]; if (__pyx_t_9 < 0) __pyx_t_9 += __pyx_v_narr2.shape[1];
((__pyx_v_bd2.coord[__pyx_v_j])[0]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr2.data + __pyx_t_8 * __pyx_v_narr2.strides[0]) ) + __pyx_t_9 * __pyx_v_narr2.strides[1]) ))); ((__pyx_v_bd2.coord[__pyx_v_j])[0]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr2.data + __pyx_t_8 * __pyx_v_narr2.strides[0]) ) + __pyx_t_9 * __pyx_v_narr2.strides[1]) )));
/* "openGJK_cython.pyx":86 /* "openGJK_cython.pyx":81
* for j in range(0, bd2.numpoints): * bd2.coord[j] = <double *> malloc(3 * sizeof(double))
* bd2.coord[j][0] = narr2[j,0] * bd2.coord[j][0] = narr2[j,0]
* bd2.coord[j][1] = narr2[j,1] # <<<<<<<<<<<<<< * bd2.coord[j][1] = narr2[j,1] # <<<<<<<<<<<<<<
* bd2.coord[j][2] = narr2[j,2] * bd2.coord[j][2] = narr2[j,2]
@ -2828,7 +2793,7 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
if (__pyx_t_8 < 0) __pyx_t_8 += __pyx_v_narr2.shape[1]; if (__pyx_t_8 < 0) __pyx_t_8 += __pyx_v_narr2.shape[1];
((__pyx_v_bd2.coord[__pyx_v_j])[1]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr2.data + __pyx_t_9 * __pyx_v_narr2.strides[0]) ) + __pyx_t_8 * __pyx_v_narr2.strides[1]) ))); ((__pyx_v_bd2.coord[__pyx_v_j])[1]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr2.data + __pyx_t_9 * __pyx_v_narr2.strides[0]) ) + __pyx_t_8 * __pyx_v_narr2.strides[1]) )));
/* "openGJK_cython.pyx":87 /* "openGJK_cython.pyx":82
* bd2.coord[j][0] = narr2[j,0] * bd2.coord[j][0] = narr2[j,0]
* bd2.coord[j][1] = narr2[j,1] * bd2.coord[j][1] = narr2[j,1]
* bd2.coord[j][2] = narr2[j,2] # <<<<<<<<<<<<<< * bd2.coord[j][2] = narr2[j,2] # <<<<<<<<<<<<<<
@ -2842,18 +2807,18 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
((__pyx_v_bd2.coord[__pyx_v_j])[2]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr2.data + __pyx_t_8 * __pyx_v_narr2.strides[0]) ) + __pyx_t_9 * __pyx_v_narr2.strides[1]) ))); ((__pyx_v_bd2.coord[__pyx_v_j])[2]) = (*((double *) ( /* dim=1 */ (( /* dim=0 */ (__pyx_v_narr2.data + __pyx_t_8 * __pyx_v_narr2.strides[0]) ) + __pyx_t_9 * __pyx_v_narr2.strides[1]) )));
} }
/* "openGJK_cython.pyx":90 /* "openGJK_cython.pyx":85
* *
* *
* print("Break 4")#-------------------------------------------------- # <<<<<<<<<<<<<< * print("Break 4")#-------------------------------------------------- # <<<<<<<<<<<<<<
* *
* # Call C function * # Call C function
*/ */
__pyx_t_5 = __Pyx_PyObject_Call(__pyx_builtin_print, __pyx_tuple__4, NULL); if (unlikely(!__pyx_t_5)) __PYX_ERR(0, 90, __pyx_L1_error) __pyx_t_5 = __Pyx_PyObject_Call(__pyx_builtin_print, __pyx_tuple__4, NULL); if (unlikely(!__pyx_t_5)) __PYX_ERR(0, 85, __pyx_L1_error)
__Pyx_GOTREF(__pyx_t_5); __Pyx_GOTREF(__pyx_t_5);
__Pyx_DECREF(__pyx_t_5); __pyx_t_5 = 0; __Pyx_DECREF(__pyx_t_5); __pyx_t_5 = 0;
/* "openGJK_cython.pyx":93 /* "openGJK_cython.pyx":88
* *
* # Call C function * # Call C function
* answer = gjk(bd1, bd2, &s) # <<<<<<<<<<<<<< * answer = gjk(bd1, bd2, &s) # <<<<<<<<<<<<<<
@ -2862,23 +2827,58 @@ static PyObject *__pyx_pf_9openGJKpy_pygjk(CYTHON_UNUSED PyObject *__pyx_self, P
*/ */
__pyx_v_answer = gjk(__pyx_v_bd1, __pyx_v_bd2, (&__pyx_v_s)); __pyx_v_answer = gjk(__pyx_v_bd1, __pyx_v_bd2, (&__pyx_v_s));
/* "openGJK_cython.pyx":96 /* "openGJK_cython.pyx":91
* *
* # Free the memory * # Free the memory
* free(bdptr) # <<<<<<<<<<<<<< * for i in range(0, bd1.numpoints): # <<<<<<<<<<<<<<
* * free(bd1.coord[i])
* return answer * for j in range(0, bd2.numpoints):
*/ */
free(__pyx_v_bdptr); __pyx_t_6 = __pyx_v_bd1.numpoints;
__pyx_t_10 = __pyx_t_6;
for (__pyx_t_11 = 0; __pyx_t_11 < __pyx_t_10; __pyx_t_11+=1) {
__pyx_v_i = __pyx_t_11;
/* "openGJK_cython.pyx":98 /* "openGJK_cython.pyx":92
* free(bdptr) * # Free the memory
* for i in range(0, bd1.numpoints):
* free(bd1.coord[i]) # <<<<<<<<<<<<<<
* for j in range(0, bd2.numpoints):
* free(bd2.coord[j])
*/
free((__pyx_v_bd1.coord[__pyx_v_i]));
}
/* "openGJK_cython.pyx":93
* for i in range(0, bd1.numpoints):
* free(bd1.coord[i])
* for j in range(0, bd2.numpoints): # <<<<<<<<<<<<<<
* free(bd2.coord[j])
*
*/
__pyx_t_6 = __pyx_v_bd2.numpoints;
__pyx_t_10 = __pyx_t_6;
for (__pyx_t_11 = 0; __pyx_t_11 < __pyx_t_10; __pyx_t_11+=1) {
__pyx_v_j = __pyx_t_11;
/* "openGJK_cython.pyx":94
* free(bd1.coord[i])
* for j in range(0, bd2.numpoints):
* free(bd2.coord[j]) # <<<<<<<<<<<<<<
*
*
*/
free((__pyx_v_bd2.coord[__pyx_v_j]));
}
/* "openGJK_cython.pyx":97
*
* *
* return answer # <<<<<<<<<<<<<< * return answer # <<<<<<<<<<<<<<
* *
*/ */
__Pyx_XDECREF(__pyx_r); __Pyx_XDECREF(__pyx_r);
__pyx_t_5 = PyFloat_FromDouble(__pyx_v_answer); if (unlikely(!__pyx_t_5)) __PYX_ERR(0, 98, __pyx_L1_error) __pyx_t_5 = PyFloat_FromDouble(__pyx_v_answer); if (unlikely(!__pyx_t_5)) __PYX_ERR(0, 97, __pyx_L1_error)
__Pyx_GOTREF(__pyx_t_5); __Pyx_GOTREF(__pyx_t_5);
__pyx_r = __pyx_t_5; __pyx_r = __pyx_t_5;
__pyx_t_5 = 0; __pyx_t_5 = 0;
@ -16529,7 +16529,6 @@ static __Pyx_StringTabEntry __pyx_string_tab[] = {
{&__pyx_n_s_base, __pyx_k_base, sizeof(__pyx_k_base), 0, 0, 1, 1}, {&__pyx_n_s_base, __pyx_k_base, sizeof(__pyx_k_base), 0, 0, 1, 1},
{&__pyx_n_s_bd1, __pyx_k_bd1, sizeof(__pyx_k_bd1), 0, 0, 1, 1}, {&__pyx_n_s_bd1, __pyx_k_bd1, sizeof(__pyx_k_bd1), 0, 0, 1, 1},
{&__pyx_n_s_bd2, __pyx_k_bd2, sizeof(__pyx_k_bd2), 0, 0, 1, 1}, {&__pyx_n_s_bd2, __pyx_k_bd2, sizeof(__pyx_k_bd2), 0, 0, 1, 1},
{&__pyx_n_s_bdptr, __pyx_k_bdptr, sizeof(__pyx_k_bdptr), 0, 0, 1, 1},
{&__pyx_n_s_bod1, __pyx_k_bod1, sizeof(__pyx_k_bod1), 0, 0, 1, 1}, {&__pyx_n_s_bod1, __pyx_k_bod1, sizeof(__pyx_k_bod1), 0, 0, 1, 1},
{&__pyx_n_s_bod2, __pyx_k_bod2, sizeof(__pyx_k_bod2), 0, 0, 1, 1}, {&__pyx_n_s_bod2, __pyx_k_bod2, sizeof(__pyx_k_bod2), 0, 0, 1, 1},
{&__pyx_n_s_c, __pyx_k_c, sizeof(__pyx_k_c), 0, 0, 1, 1}, {&__pyx_n_s_c, __pyx_k_c, sizeof(__pyx_k_c), 0, 0, 1, 1},
@ -16608,7 +16607,7 @@ static __Pyx_StringTabEntry __pyx_string_tab[] = {
}; };
static CYTHON_SMALL_CODE int __Pyx_InitCachedBuiltins(void) { static CYTHON_SMALL_CODE int __Pyx_InitCachedBuiltins(void) {
__pyx_builtin_print = __Pyx_GetBuiltinName(__pyx_n_s_print); if (!__pyx_builtin_print) __PYX_ERR(0, 34, __pyx_L1_error) __pyx_builtin_print = __Pyx_GetBuiltinName(__pyx_n_s_print); if (!__pyx_builtin_print) __PYX_ERR(0, 34, __pyx_L1_error)
__pyx_builtin_range = __Pyx_GetBuiltinName(__pyx_n_s_range); if (!__pyx_builtin_range) __PYX_ERR(0, 78, __pyx_L1_error) __pyx_builtin_range = __Pyx_GetBuiltinName(__pyx_n_s_range); if (!__pyx_builtin_range) __PYX_ERR(0, 71, __pyx_L1_error)
__pyx_builtin_ValueError = __Pyx_GetBuiltinName(__pyx_n_s_ValueError); if (!__pyx_builtin_ValueError) __PYX_ERR(1, 133, __pyx_L1_error) __pyx_builtin_ValueError = __Pyx_GetBuiltinName(__pyx_n_s_ValueError); if (!__pyx_builtin_ValueError) __PYX_ERR(1, 133, __pyx_L1_error)
__pyx_builtin_MemoryError = __Pyx_GetBuiltinName(__pyx_n_s_MemoryError); if (!__pyx_builtin_MemoryError) __PYX_ERR(1, 148, __pyx_L1_error) __pyx_builtin_MemoryError = __Pyx_GetBuiltinName(__pyx_n_s_MemoryError); if (!__pyx_builtin_MemoryError) __PYX_ERR(1, 148, __pyx_L1_error)
__pyx_builtin_enumerate = __Pyx_GetBuiltinName(__pyx_n_s_enumerate); if (!__pyx_builtin_enumerate) __PYX_ERR(1, 151, __pyx_L1_error) __pyx_builtin_enumerate = __Pyx_GetBuiltinName(__pyx_n_s_enumerate); if (!__pyx_builtin_enumerate) __PYX_ERR(1, 151, __pyx_L1_error)
@ -16636,36 +16635,36 @@ static CYTHON_SMALL_CODE int __Pyx_InitCachedConstants(void) {
__Pyx_GOTREF(__pyx_tuple_); __Pyx_GOTREF(__pyx_tuple_);
__Pyx_GIVEREF(__pyx_tuple_); __Pyx_GIVEREF(__pyx_tuple_);
/* "openGJK_cython.pyx":66 /* "openGJK_cython.pyx":59
* *
* *
* print("Break 2")#-------------------------------------------------- # <<<<<<<<<<<<<< * print("Break 2")#-------------------------------------------------- # <<<<<<<<<<<<<<
* *
* # Create numpy-array MemoryView * # Create numpy-array MemoryView
*/ */
__pyx_tuple__2 = PyTuple_Pack(1, __pyx_kp_u_Break_2); if (unlikely(!__pyx_tuple__2)) __PYX_ERR(0, 66, __pyx_L1_error) __pyx_tuple__2 = PyTuple_Pack(1, __pyx_kp_u_Break_2); if (unlikely(!__pyx_tuple__2)) __PYX_ERR(0, 59, __pyx_L1_error)
__Pyx_GOTREF(__pyx_tuple__2); __Pyx_GOTREF(__pyx_tuple__2);
__Pyx_GIVEREF(__pyx_tuple__2); __Pyx_GIVEREF(__pyx_tuple__2);
/* "openGJK_cython.pyx":75 /* "openGJK_cython.pyx":68
* print(narr2[0,0]) # output a <double>, works fine * print(narr2[0,0]) # output a <double>, works fine
* *
* print("Break 3")#-------------------------------------------------- # <<<<<<<<<<<<<< * print("Break 3")#-------------------------------------------------- # <<<<<<<<<<<<<<
* *
* # Assign coordinate values (Segmentation Fault Here!!, ) * # Assign coordinate values (Segmentation Fault Here!!, )
*/ */
__pyx_tuple__3 = PyTuple_Pack(1, __pyx_kp_u_Break_3); if (unlikely(!__pyx_tuple__3)) __PYX_ERR(0, 75, __pyx_L1_error) __pyx_tuple__3 = PyTuple_Pack(1, __pyx_kp_u_Break_3); if (unlikely(!__pyx_tuple__3)) __PYX_ERR(0, 68, __pyx_L1_error)
__Pyx_GOTREF(__pyx_tuple__3); __Pyx_GOTREF(__pyx_tuple__3);
__Pyx_GIVEREF(__pyx_tuple__3); __Pyx_GIVEREF(__pyx_tuple__3);
/* "openGJK_cython.pyx":90 /* "openGJK_cython.pyx":85
* *
* *
* print("Break 4")#-------------------------------------------------- # <<<<<<<<<<<<<< * print("Break 4")#-------------------------------------------------- # <<<<<<<<<<<<<<
* *
* # Call C function * # Call C function
*/ */
__pyx_tuple__4 = PyTuple_Pack(1, __pyx_kp_u_Break_4); if (unlikely(!__pyx_tuple__4)) __PYX_ERR(0, 90, __pyx_L1_error) __pyx_tuple__4 = PyTuple_Pack(1, __pyx_kp_u_Break_4); if (unlikely(!__pyx_tuple__4)) __PYX_ERR(0, 85, __pyx_L1_error)
__Pyx_GOTREF(__pyx_tuple__4); __Pyx_GOTREF(__pyx_tuple__4);
__Pyx_GIVEREF(__pyx_tuple__4); __Pyx_GIVEREF(__pyx_tuple__4);
@ -16868,10 +16867,10 @@ static CYTHON_SMALL_CODE int __Pyx_InitCachedConstants(void) {
* *
* # Declare data types * # Declare data types
*/ */
__pyx_tuple__23 = PyTuple_Pack(11, __pyx_n_s_bod1, __pyx_n_s_bod2, __pyx_n_s_s, __pyx_n_s_bd1, __pyx_n_s_bd2, __pyx_n_s_i, __pyx_n_s_j, __pyx_n_s_answer, __pyx_n_s_bdptr, __pyx_n_s_narr1, __pyx_n_s_narr2); if (unlikely(!__pyx_tuple__23)) __PYX_ERR(0, 24, __pyx_L1_error) __pyx_tuple__23 = PyTuple_Pack(10, __pyx_n_s_bod1, __pyx_n_s_bod2, __pyx_n_s_s, __pyx_n_s_bd1, __pyx_n_s_bd2, __pyx_n_s_i, __pyx_n_s_j, __pyx_n_s_answer, __pyx_n_s_narr1, __pyx_n_s_narr2); if (unlikely(!__pyx_tuple__23)) __PYX_ERR(0, 24, __pyx_L1_error)
__Pyx_GOTREF(__pyx_tuple__23); __Pyx_GOTREF(__pyx_tuple__23);
__Pyx_GIVEREF(__pyx_tuple__23); __Pyx_GIVEREF(__pyx_tuple__23);
__pyx_codeobj__24 = (PyObject*)__Pyx_PyCode_New(2, 0, 11, 0, CO_OPTIMIZED|CO_NEWLOCALS, __pyx_empty_bytes, __pyx_empty_tuple, __pyx_empty_tuple, __pyx_tuple__23, __pyx_empty_tuple, __pyx_empty_tuple, __pyx_kp_s_openGJK_cython_pyx, __pyx_n_s_pygjk, 24, __pyx_empty_bytes); if (unlikely(!__pyx_codeobj__24)) __PYX_ERR(0, 24, __pyx_L1_error) __pyx_codeobj__24 = (PyObject*)__Pyx_PyCode_New(2, 0, 10, 0, CO_OPTIMIZED|CO_NEWLOCALS, __pyx_empty_bytes, __pyx_empty_tuple, __pyx_empty_tuple, __pyx_tuple__23, __pyx_empty_tuple, __pyx_empty_tuple, __pyx_kp_s_openGJK_cython_pyx, __pyx_n_s_pygjk, 24, __pyx_empty_bytes); if (unlikely(!__pyx_codeobj__24)) __PYX_ERR(0, 24, __pyx_L1_error)
/* "View.MemoryView":286 /* "View.MemoryView":286
* return self.name * return self.name

19
Cython/openGJK_cython.pyx
View File

@ -52,15 +52,8 @@ def pygjk(bod1, bod2):
# Allocate memory for pointer (not working) # Allocate memory for pointer (not working)
cdef bd *bdptr = <bd *> malloc(sizeof(bd)) bd1.coord = <double **> malloc(bd1.numpoints * sizeof(double *))
bd2.coord = <double **> malloc(bd2.numpoints * sizeof(double *))
if bd1.numpoints > bd2.numpoints:
bdptr.coord[0] = <double *> malloc(3 * sizeof(double))
bdptr.coord = <double **> malloc(bd1.numpoints * sizeof(bdptr.coord[0]))
else:
bdptr.coord[0] = <double *> malloc(3 * sizeof(double))
bdptr.coord = <double **> malloc(bd2.numpoints * sizeof(bdptr.coord[0]))
print("Break 2")#-------------------------------------------------- print("Break 2")#--------------------------------------------------
@ -76,12 +69,14 @@ def pygjk(bod1, bod2):
# Assign coordinate values (Segmentation Fault Here!!, ) # Assign coordinate values (Segmentation Fault Here!!, )
for i in range(0, bd1.numpoints): for i in range(0, bd1.numpoints):
bd1.coord[i] = <double *> malloc(3 * sizeof(double))
bd1.coord[i][0] = narr1[i,0] bd1.coord[i][0] = narr1[i,0]
bd1.coord[i][1] = narr1[i,1] bd1.coord[i][1] = narr1[i,1]
bd1.coord[i][2] = narr1[i,2] bd1.coord[i][2] = narr1[i,2]
for j in range(0, bd2.numpoints): for j in range(0, bd2.numpoints):
bd2.coord[j] = <double *> malloc(3 * sizeof(double))
bd2.coord[j][0] = narr2[j,0] bd2.coord[j][0] = narr2[j,0]
bd2.coord[j][1] = narr2[j,1] bd2.coord[j][1] = narr2[j,1]
bd2.coord[j][2] = narr2[j,2] bd2.coord[j][2] = narr2[j,2]
@ -93,7 +88,11 @@ def pygjk(bod1, bod2):
answer = gjk(bd1, bd2, &s) answer = gjk(bd1, bd2, &s)
# Free the memory # Free the memory
free(bdptr) for i in range(0, bd1.numpoints):
free(bd1.coord[i])
for j in range(0, bd2.numpoints):
free(bd2.coord[j])
return answer return answer

248
Cython/test.py
View File

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