Merge branch 'master' into add_github_action

fixes-turtlebasket
Mattia Montanari 2023-02-14 14:01:39 +01:00
commit e0a9626943
5 changed files with 135 additions and 0 deletions

36
examples/go/example.go Normal file
View File

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

3
examples/go/go.work Normal file
View File

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

View File

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

View File

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

View File

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