Merge pull request #39 from aturkenov/feature/go-example

go example

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)
+	}
+}

examples/go/go.work

@@ -0,0 +1,3 @@
+go 1.18
+
+use openGJK

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)
+}

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)
+}

examples/go/openGJK/go.mod

@@ -0,0 +1,3 @@
+module github.com/MattiaMontanari/openGJK/examples/go/openGJK
+
+go 1.18