openGJK/Cython/openGJK_cython.pyx

96 lines
2.1 KiB
Cython
Raw Normal View History

2020-05-18 16:54:28 -07:00
#cython: language_level=3, boundscheck=False
2020-05-19 17:46:42 -07:00
import numpy as np
from libc.stdlib cimport free, malloc
from cpython.mem cimport PyMem_Malloc, PyMem_Free
2020-05-19 17:46:42 -07:00
2020-05-21 06:08:37 -07:00
# Declare C function and data types
2020-05-18 16:54:28 -07:00
cdef extern from "openGJK.h":
struct bd:
int numpoints
double s[3]
double ** coord
2020-05-18 16:54:28 -07:00
struct simplex:
int nvrtx
double vrtx[4][3]
int wids[4]
double lambdas[4]
double gjk(bd bd1, bd bd2, simplex *s)
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-19 09:44:27 -07:00
double answer
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)
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)
bd2.numpoints = np.size(bod2,0) - 1
else:
bd2.numpoints = np.size(bod2,0)
2020-05-18 16:54:28 -07:00
# Allocate memory for pointer (not working)
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 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-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 04:30:33 -07:00
bd1.coord[i] = <double *> malloc(3 * sizeof(double))
2020-05-21 06:08:37 -07:00
if not bd1.coord[i]:
raise NameError('Not enough memory for bd1.coord[]')
2020-05-19 17:46:42 -07:00
bd1.coord[i][0] = narr1[i,0]
bd1.coord[i][1] = narr1[i,1]
bd1.coord[i][2] = narr1[i,2]
2020-05-19 09:44:27 -07:00
2020-05-18 16:54:28 -07:00
for j in range(0, bd2.numpoints):
2020-05-21 04:30:33 -07:00
bd2.coord[j] = <double *> malloc(3 * sizeof(double))
2020-05-21 06:08:37 -07:00
if not bd2.coord[j]:
raise NameError('Not enough memory for bd2.coord[]')
2020-05-19 17:46:42 -07:00
bd2.coord[j][0] = narr2[j,0]
bd2.coord[j][1] = narr2[j,1]
bd2.coord[j][2] = narr2[j,2]
2020-05-19 09:44:27 -07:00
2020-05-19 17:46:42 -07:00
# Call C function
answer = gjk(bd1, bd2, &s)
# 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-18 16:54:28 -07:00
2020-05-19 09:44:27 -07:00
return answer
2020-05-18 16:54:28 -07:00