2020-05-21 09:13:21 -07:00
|
|
|
#!python
|
|
|
|
# cython: language_level=3
|
2020-05-21 09:20:33 -07:00
|
|
|
# distutils: sources = ../../src/openGJK.c
|
|
|
|
# distutils: include_dirs = ../../include/openGJK
|
2020-05-21 08:24:36 -07:00
|
|
|
|
|
|
|
cimport openGJK_cython
|
2020-05-18 16:54:28 -07:00
|
|
|
|
2020-05-19 17:46:42 -07:00
|
|
|
import numpy as np
|
|
|
|
from libc.stdlib cimport free, malloc
|
2020-05-21 03:59:32 -07:00
|
|
|
from cpython.mem cimport PyMem_Malloc, PyMem_Free
|
2020-05-19 17:46:42 -07:00
|
|
|
|
|
|
|
# Create Python function
|
2020-05-18 16:54:28 -07:00
|
|
|
def pygjk(bod1, bod2):
|
|
|
|
|
2020-05-19 17:46:42 -07:00
|
|
|
# Declare data types
|
2020-05-18 16:54:28 -07:00
|
|
|
cdef:
|
2020-05-19 17:46:42 -07:00
|
|
|
simplex s
|
2020-05-18 16:54:28 -07:00
|
|
|
bd bd1
|
|
|
|
bd bd2
|
2020-05-21 07:46:10 -07:00
|
|
|
double dist2
|
2020-05-18 16:54:28 -07:00
|
|
|
|
2020-05-19 17:46:42 -07:00
|
|
|
# Convert 1D array to 2D, if any
|
2020-05-19 09:44:27 -07:00
|
|
|
if bod1.ndim < 2:
|
|
|
|
bod1 = np.append([bod1], [[1.,1.,1.]], axis = 0)
|
2020-05-21 03:59:32 -07:00
|
|
|
bd1.numpoints = np.size(bod1,0) - 1
|
|
|
|
else:
|
|
|
|
bd1.numpoints = np.size(bod1,0)
|
|
|
|
|
|
|
|
|
2020-05-19 09:44:27 -07:00
|
|
|
if bod2.ndim < 2:
|
|
|
|
bod2 = np.append([bod2], [[1.,1.,1.]], axis = 0)
|
2020-05-21 03:59:32 -07:00
|
|
|
bd2.numpoints = np.size(bod2,0) - 1
|
|
|
|
else:
|
|
|
|
bd2.numpoints = np.size(bod2,0)
|
2020-05-18 16:54:28 -07:00
|
|
|
|
2020-05-21 03:59:32 -07:00
|
|
|
|
2020-05-21 07:46:10 -07:00
|
|
|
# Allocate memory for bodies
|
2020-05-21 04:30:33 -07:00
|
|
|
bd1.coord = <double **> malloc(bd1.numpoints * sizeof(double *))
|
2020-05-21 06:08:37 -07:00
|
|
|
if not bd1.coord:
|
|
|
|
raise NameError('Not enough memory for bd1.coord')
|
2020-05-21 07:46:10 -07:00
|
|
|
for i in range(0, bd1.numpoints):
|
|
|
|
bd1.coord[i] = <double *> malloc(3 * sizeof(double))
|
|
|
|
if not bd1.coord[i]:
|
|
|
|
raise NameError('Not enough memory for bd1.coord[]')
|
|
|
|
|
2020-05-21 04:30:33 -07:00
|
|
|
bd2.coord = <double **> malloc(bd2.numpoints * sizeof(double *))
|
2020-05-21 06:08:37 -07:00
|
|
|
if not bd2.coord:
|
|
|
|
raise NameError('Not enough memory for bd2.coord')
|
2020-05-21 07:46:10 -07:00
|
|
|
for j in range(0, bd2.numpoints):
|
|
|
|
bd2.coord[j] = <double *> malloc(3 * sizeof(double))
|
|
|
|
if not bd2.coord[j]:
|
|
|
|
raise NameError('Not enough memory for bd2.coord[]')
|
2020-05-19 17:46:42 -07:00
|
|
|
|
|
|
|
# Create numpy-array MemoryView
|
2020-05-19 09:44:27 -07:00
|
|
|
cdef:
|
2020-05-19 17:46:42 -07:00
|
|
|
double [:,:] narr1 = bod1
|
2020-05-19 09:44:27 -07:00
|
|
|
double [:,:] narr2 = bod2
|
|
|
|
|
2020-05-21 06:08:37 -07:00
|
|
|
# Assign coordinate values
|
2020-05-18 16:54:28 -07:00
|
|
|
for i in range(0, bd1.numpoints):
|
2020-05-21 07:46:10 -07:00
|
|
|
for j in range(0,3):
|
|
|
|
bd1.coord[i][j] = narr1[i,j]
|
2020-05-18 16:54:28 -07:00
|
|
|
|
2020-05-21 07:46:10 -07:00
|
|
|
for i in range(0, bd2.numpoints):
|
|
|
|
for j in range(0,3):
|
|
|
|
bd2.coord[i][j] = narr2[i,j]
|
2020-05-19 09:44:27 -07:00
|
|
|
|
2020-05-19 17:46:42 -07:00
|
|
|
# Call C function
|
2020-05-21 07:46:10 -07:00
|
|
|
dist2 = gjk(bd1, bd2, &s)
|
2020-05-19 17:46:42 -07:00
|
|
|
|
|
|
|
# Free the memory
|
2020-05-21 06:08:37 -07:00
|
|
|
for ii in range(0, bd1.numpoints):
|
|
|
|
free(bd1.coord[ii])
|
|
|
|
free(bd1.coord)
|
|
|
|
|
|
|
|
for jj in range(0, bd2.numpoints):
|
|
|
|
free(bd2.coord[jj])
|
|
|
|
free(bd2.coord)
|
2020-05-21 04:30:33 -07:00
|
|
|
|
2020-05-21 07:46:10 -07:00
|
|
|
return dist2
|
2020-05-18 16:54:28 -07:00
|
|
|
|