diff --git a/examples/go/example.go b/examples/go/example.go new file mode 100644 index 0000000..b886260 --- /dev/null +++ b/examples/go/example.go @@ -0,0 +1,36 @@ +package main + +import ( + "fmt" + + "github.com/MattiaMontanari/openGJK/examples/go/openGJK" +) + +func main() { + a := [][3]float64{ + {-1.0, -1.0, 0.0}, + {-1.0, 1.0, 0.0}, + {1.0, 1.0, 0.0}, + {1.0, -1.0, 0.0}, + } + b := [][3]float64{ + {0.0, -0.5, 0.0}, + {0.0, 0.5, 0.0}, + {2.0, 0.5, 0.0}, + {2.0, -0.5, 0.0}, + } + c := [][3]float64{ + {3.0, -0.5, 0.0}, + {3.0, 0.5, 0.0}, + {5.0, 0.5, 0.0}, + {5.0, -0.5, 0.0}, + } + collided := openGJK.GJK(a, b) + if collided == 0 { + fmt.Println("a and b is collided") + } + distance := openGJK.GJK(a, c) + if distance > 0 { + fmt.Println("distance from a to c is", distance) + } +} diff --git a/examples/go/go.work b/examples/go/go.work new file mode 100644 index 0000000..244d399 --- /dev/null +++ b/examples/go/go.work @@ -0,0 +1,3 @@ +go 1.18 + +use openGJK diff --git a/examples/go/openGJK/connector.go b/examples/go/openGJK/connector.go new file mode 100644 index 0000000..a0bb7ce --- /dev/null +++ b/examples/go/openGJK/connector.go @@ -0,0 +1,60 @@ +package openGJK + +/* +#cgo CFLAGS: -I../../../include/ -I../../../ +#cgo LDFLAGS: -lm +#include "openGJK.c" +void __array_to_matrix(double* input, unsigned long length, double* matrix[]) { + for (int i = 0; i < length; i++) { + double* row[3]; + for (int j = 0; j < 3; j++) { + row[j] = &input[i*3+j]; + } + matrix[i] = row[0]; + } +} +double __gjk(double *a, unsigned long a_length, double *b, unsigned long b_length) { + gkSimplex simplex; + gkPolytope a_polytope; + gkPolytope b_polytope; + double* a_matrix[a_length]; + double* b_matrix[b_length]; + __array_to_matrix(a, a_length, a_matrix); + __array_to_matrix(b, b_length, b_matrix); + a_polytope.coord = a_matrix; + b_polytope.coord = b_matrix; + a_polytope.numpoints = a_length; + b_polytope.numpoints = b_length; + double distance = compute_minimum_distance(a_polytope, b_polytope, &simplex); + return distance; +} +*/ +import "C" +import "unsafe" + +// Converts nx3 matrix to n*3 array of C.double type +// Returns pointer to array first element +func matrix_to_carray(data [][3]float64) unsafe.Pointer { + array := []C.double{} + row_length := len(data) + i := 0 + for i < row_length { + j := 0 + for j < 3 { + array = append(array, C.double(data[i][j])) + j += 1 + } + i += 1 + } + return unsafe.Pointer(&array[0]) +} + +// Compute minimum distance of two objects +func GJK(a [][3]float64, b [][3]float64) float64 { + pa := matrix_to_carray(a) + pb := matrix_to_carray(b) + na := C.ulong(len(a)) + nb := C.ulong(len(b)) + cdistance := C.__gjk((*C.double)(pa), na, (*C.double)(pb), nb) + return float64(cdistance) +} diff --git a/examples/go/openGJK/connector_test.go b/examples/go/openGJK/connector_test.go new file mode 100644 index 0000000..2feaf96 --- /dev/null +++ b/examples/go/openGJK/connector_test.go @@ -0,0 +1,33 @@ +package openGJK + +import "testing" + +func fassert(t *testing.T, got, want float64) { + if got != want { + t.Errorf("GJK(b, c) -> %f, want %f", got, want) + } +} + +func TestGJK(t *testing.T) { + a := [][3]float64{ + {-1.0, -1.0, 0.0}, + {-1.0, 1.0, 0.0}, + {1.0, 1.0, 0.0}, + {1.0, -1.0, 0.0}, + } + b := [][3]float64{ + {0.0, -0.5, 0.0}, + {0.0, 0.5, 0.0}, + {2.0, 0.5, 0.0}, + {2.0, -0.5, 0.0}, + } + c := [][3]float64{ + {3.0, -0.5, 0.0}, + {3.0, 0.5, 0.0}, + {5.0, 0.5, 0.0}, + {5.0, -0.5, 0.0}, + } + fassert(t, GJK(a, b), 0) + fassert(t, GJK(b, c), 1) + fassert(t, GJK(a, c), 2) +} diff --git a/examples/go/openGJK/go.mod b/examples/go/openGJK/go.mod new file mode 100644 index 0000000..8a2bc3d --- /dev/null +++ b/examples/go/openGJK/go.mod @@ -0,0 +1,3 @@ +module github.com/MattiaMontanari/openGJK/examples/go/openGJK + +go 1.18