turtlebasket/openGJK
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Minalistic community edition (CE) library

Browse Source
This commit is contained in:
Mattia Montanari
2022-07-09 23:10:45 +02:00
parent 7471aa01e1
commit d8c123c434
29 changed files with 1575 additions and 7328 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
examples/c/CMakeLists.txt
View File

@@ -1,67 +1,40 @@
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
# ##### # # # #
# #### ##### ###### # # # # # # # #
# # # # # # ## # # # # # #
# # # # # ##### # # # # #### # ### #
# # # ##### # # # # # # # # # # #
# # # # # # ## # # # # # # #
# #### # ###### # # ##### ##### # # #
# #
# This file is part of openGJK. #
# #
# openGJK is free software: you can redistribute it and/or modify #
# it under the terms of the GNU General Public License as published by #
# the Free Software Foundation, either version 3 of the License, or #
# any later version. #
# #
# openGJK is distributed in the hope that it will be useful, #
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The #
# GNU General Public License for more details. #
# #
# You should have received a copy of the GNU General Public License #
# along with openGJK. If not, see <https://www.gnu.org/licenses/>. #
# #
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
# Copyright (C) Mattia Montanari 2018 - 2019 #
# http://iel.eng.ox.ac.uk/?page_id=504 #
# #
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
project(openGJKdemo VERSION 1.0.0 LANGUAGES C)
set(APPLICATION_NAME ${PROJECT_NAME})
set(CMAKE_C_STANDARD 11)
set(TEST_NAME ${PROJECT_NAME}_CTEST)
message( "[${PROJECT_NAME}] CMake setting ..")
# Set source file
set(SOURCE_FILES main.c )
# Create the executable
add_executable(demo ${SOURCE_FILES})
# Copy input files for this example after build
add_custom_command(
TARGET demo POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy
${CMAKE_CURRENT_SOURCE_DIR}/userP.dat
${CMAKE_CURRENT_BINARY_DIR}/userP.dat )
add_custom_command(
TARGET demo POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy
${CMAKE_CURRENT_SOURCE_DIR}/userQ.dat
${CMAKE_CURRENT_BINARY_DIR}/userQ.dat )
# PLATFORM-SPECIFIC SETTING
if (UNIX)
find_library(M_LIB m)
# Link to openGJK and math library
target_link_libraries(demo openGJKlib m)
else ()
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
target_link_libraries(demo openGJKlib)
endif ()
message(STATUS "Completed CMake setting for ${PROJECT_NAME}" )
# _____ _ _ __ #
# / ____| | | |/ / #
# ___ _ __ ___ _ __ | | __ | | ' / #
# / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < #
# | (_) | |_) | __/ | | | |__| | |__| | . \ #
# \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ #
# | | #
# |_| #
# #
# Copyright 2022 Mattia Montanari, University of Oxford #
# #
# This program is free software: you can redistribute it and/or modify it under #
# the terms of the GNU General Public License as published by the Free Software #
# Foundation, either version 3 of the License. You should have received a copy #
# of the GNU General Public License along with this program. If not, visit #
# #
# https://www.gnu.org/licenses/ #
# #
# This program is distributed in the hope that it will be useful, but WITHOUT #
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS #
# FOR A PARTICULAR PURPOSE. See GNU General Public License for details. #
project(example_lib_opengjk_ce
LANGUAGES C
VERSION 1.0.0
)
add_executable(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/main.c)
target_link_libraries(${PROJECT_NAME} lib_opengjk_ce)
# Copy input files for this example after build
add_custom_command(
TARGET ${PROJECT_NAME} POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy
${CMAKE_CURRENT_SOURCE_DIR}/userP.dat
${CMAKE_CURRENT_BINARY_DIR}/userP.dat
COMMAND ${CMAKE_COMMAND} -E copy
${CMAKE_CURRENT_SOURCE_DIR}/userQ.dat
${CMAKE_CURRENT_BINARY_DIR}/userQ.dat
)

307
examples/c/main.c
View File

@@ -1,178 +1,129 @@
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* This file is part of openGJK. *
* *
* openGJK is free software: you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation, either version 3 of the License, or *
* any later version. *
* *
* openGJK is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
* *
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
* Copyright (C) Mattia Montanari 2018 - 2019 *
* http://iel.eng.ox.ac.uk/?page_id=504 *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* *
* This file runs an example to illustrate how to invoke the openGJK lib *
* within a C program. An executable called 'demo' can be compiled with *
* CMake. This reads the coordinates of two polytopes from the input *
* files userP.dat and userQ.dat, respectively, and returns the minimum *
* distance between them computed using the openGJK library. *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/**
* @file main.c
* @author Mattia Montanari
* @date April 2018
* @brief File illustrating an application that invokes openGJK.
*
*/
#define _CRT_HAS_CXX17 0
#include <stdlib.h>
#include <stdio.h>
/* For importing openGJK this is Step 1: include header in subfolder. */
#include "openGJK/openGJK.h"
#ifndef WIN32
#define fscanf_s fscanf
#endif
/**
* @brief Function for reading input file with body's coordinates.
*
*/
int readinput(const char *inputfile, double ***pts, int * out) {
int npoints = 0;
int idx = 0;
FILE *fp;
/* Open file. */
#ifdef WIN32
errno_t err;
if ((err = fopen_s(&fp, inputfile, "r")) != 0) {
#else
if ((fp = fopen(inputfile, "r")) == NULL) {
#endif
fprintf(stdout, "ERROR: input file %s not found!\n", inputfile);
fprintf(stdout, " -> The file must be in the folder from which this program is launched\n\n");
return 1;
}
/* Read number of input vertices. */
if (fscanf(fp, "%d", &npoints) != 1)
return 1;
/* Allocate memory. */
double **arr = (double **)malloc(npoints * sizeof(double *));
for (int i = 0; i < npoints; i++)
arr[i] = (double *)malloc(3 * sizeof(double));
/* Read and store vertices' coordinates. */
for (idx = 0; idx < npoints; idx++)
{
if (fscanf(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3)
return 1;
}
/* Close file. */
fclose(fp);
/* Pass pointers. */
*pts = arr;
*out = idx;
return (0);
}
/**
* @brief Main program of example1_c (described in Section 3.1 of the paper).
*
*/
int main() {
/* Squared distance computed by openGJK. */
double dd;
/* Structure of simplex used by openGJK. */
struct simplex s;
/* Number of vertices defining body 1 and body 2, respectively. */
int nvrtx1,
nvrtx2;
/* Structures of body 1 and body 2, respectively. */
struct bd bd1;
struct bd bd2;
/* Specify name of input files for body 1 and body 2, respectively. */
char inputfileA[40] = "userP.dat",
inputfileB[40] = "userQ.dat";
/* Pointers to vertices' coordinates of body 1 and body 2, respectively. */
double(**vrtx1) = NULL,
(**vrtx2) = NULL;
/* For importing openGJK this is Step 2: adapt the data structure for the
* two bodies that will be passed to the GJK procedure. */
/* Import coordinates of object 1. */
if (readinput(inputfileA, &vrtx1, &nvrtx1))
return (1);
bd1.coord = vrtx1;
bd1.numpoints = nvrtx1;
/* Import coordinates of object 2. */
if (readinput(inputfileB, &vrtx2, &nvrtx2))
return (1);
bd2.coord = vrtx2;
bd2.numpoints = nvrtx2;
/* Initialise simplex as empty */
s.nvrtx = 0;
#ifdef DEBUG
/* Verify input of body A. */
for (int i = 0; i < bd1.numpoints; ++i) {
printf("%.2f ", vrtx1[i][0]);
printf("%.2f ", vrtx1[i][1]);
printf("%.2f\n", bd1.coord[i][2]);
}
/* Verify input of body B. */
for (int i = 0; i < bd2.numpoints; ++i) {
printf("%.2f ", bd2.coord[i][0]);
printf("%.2f ", bd2.coord[i][1]);
printf("%.2f\n", bd2.coord[i][2]);
}
#endif
/* For importing openGJK this is Step 3: invoke the GJK procedure. */
/* Compute squared distance using GJK algorithm. */
dd = gjk(bd1, bd2, &s);
/* Print distance between objects. */
printf("Distance between bodies %f\n", dd);
/* Free memory */
for (int i = 0; i < bd1.numpoints; i++)
free(bd1.coord[i]);
free(bd1.coord);
for (int i = 0; i < bd2.numpoints; i++)
free(bd2.coord[i]);
free(bd2.coord);
return (0);
}
// _____ _ _ __ //
// / ____| | | |/ / //
// ___ _ __ ___ _ __ | | __ | | ' / //
// / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < //
// | (_) | |_) | __/ | | | |__| | |__| | . \ //
// \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ //
// | | //
// |_| //
// //
// Copyright 2022 Mattia Montanari, University of Oxford //
// //
// This program is free software: you can redistribute it and/or modify it under //
// the terms of the GNU General Public License as published by the Free Software //
// Foundation, either version 3 of the License. You should have received a copy //
// of the GNU General Public License along with this program. If not, visit //
// //
// https://www.gnu.org/licenses/ //
// //
// This program is distributed in the hope that it will be useful, but WITHOUT //
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS //
// FOR A PARTICULAR PURPOSE. See GNU General Public License for details. //
/// @author Mattia Montanari
/// @date July 2022
#include <stdio.h>
#include <stdlib.h>
#include "openGJK/openGJK.h"
#define fscanf_s fscanf
/// @brief Function for reading input file with body's coordinates.
int readinput(const char *inputfile, double ***pts, int *out) {
int npoints = 0;
int idx = 0;
FILE *fp;
/* Open file. */
#ifdef WIN32
errno_t err;
if ((err = fopen_s(&fp, inputfile, "r")) != 0) {
#else
if ((fp = fopen(inputfile, "r")) == NULL) {
#endif
fprintf(stdout, "ERROR: input file %s not found!\n", inputfile);
fprintf(stdout, " -> The file must be in the folder from which this "
"program is launched\n\n");
return 1;
}
/* Read number of input vertices. */
if (fscanf_s(fp, "%d", &npoints) != 1)
return 1;
/* Allocate memory. */
double **arr = (double **)malloc(npoints * sizeof(double *));
for (int i = 0; i < npoints; i++)
arr[i] = (double *)malloc(3 * sizeof(double));
/* Read and store vertices' coordinates. */
for (idx = 0; idx < npoints; idx++) {
if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) !=
3)
return 1;
}
fclose(fp);
*pts = arr;
*out = idx;
return (0);
}
/**
* @brief Main program of example1_c (described in Section 3.1 of the paper).
*
*/
int main() {
/* Squared distance computed by openGJK. */
double dd;
/* Structure of simplex used by openGJK. */
gkSimplex s;
/* Number of vertices defining body 1 and body 2, respectively. */
int nvrtx1, nvrtx2;
/* Structures of body 1 and body 2, respectively. */
gkPolytope bd1;
gkPolytope bd2;
/* Specify name of input files for body 1 and body 2, respectively. */
char inputfileA[40] = "userP.dat", inputfileB[40] = "userQ.dat";
/* Pointers to vertices' coordinates of body 1 and body 2, respectively. */
double(**vrtx1) = NULL, (**vrtx2) = NULL;
/* For importing openGJK this is Step 2: adapt the data structure for the
* two bodies that will be passed to the GJK procedure. */
/* Import coordinates of object 1. */
if (readinput(inputfileA, &vrtx1, &nvrtx1))
return (1);
bd1.coord = vrtx1;
bd1.numpoints = nvrtx1;
/* Import coordinates of object 2. */
if (readinput(inputfileB, &vrtx2, &nvrtx2))
return (1);
bd2.coord = vrtx2;
bd2.numpoints = nvrtx2;
/* Initialise simplex as empty */
s.nvrtx = 0;
/* For importing openGJK this is Step 3: invoke the GJK procedure. */
/* Compute squared distance using GJK algorithm. */
dd = compute_minimum_distance(bd1, bd2, &s);
/* Print distance between objects. */
printf("Distance between bodies %f\n", dd);
/* Free memory */
for (int i = 0; i < bd1.numpoints; i++)
free(bd1.coord[i]);
free(bd1.coord);
for (int i = 0; i < bd2.numpoints; i++)
free(bd2.coord[i]);
free(bd2.coord);
return (0);
}

20
examples/c/userP.dat
View File

@@ -1,10 +1,10 @@
9
0.0 5.5 0.0
2.3 1.0 -2.0
8.1 4.0 2.4
4.3 5.0 2.2
2.5 1.0 2.3
7.1 1.0 2.4
1.0 1.5 0.3
3.3 0.5 0.3
6.0 1.4 0.2
9
0.0 5.5 0.0
2.3 1.0 -2.0
8.1 4.0 2.4
4.3 5.0 2.2
2.5 1.0 2.3
7.1 1.0 2.4
1.0 1.5 0.3
3.3 0.5 0.3
6.0 1.4 0.2

20
examples/c/userQ.dat
View File

@@ -1,10 +1,10 @@
9
-0.0 -5.5 0.0
-2.3 -1.0 2.0
-8.1 -4.0 -2.4
-4.3 -5.0 -2.2
-2.5 -1.0 -2.3
-7.1 -1.0 -2.4
-1.0 -1.5 -0.3
-3.3 -0.5 -0.3
-6.0 -1.4 -0.2
9
-0.0 -5.5 0.0
-2.3 -1.0 2.0
-8.1 -4.0 -2.4
-4.3 -5.0 -2.2
-2.5 -1.0 -2.3
-7.1 -1.0 -2.4
-1.0 -1.5 -0.3
-3.3 -0.5 -0.3
-6.0 -1.4 -0.2

126
examples/cs/main.cs
View File

@@ -1,63 +1,63 @@
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* This file is part of openGJK. *
* *
* openGJK is free software: you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation, either version 3 of the License, or *
* any later version. *
* *
* openGJK is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
* *
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
* Copyright (C) Mattia Montanari 2018 - 2019 *
* http://iel.eng.ox.ac.uk/?page_id=504 *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
using System;
using System.Runtime.InteropServices;
public class Tester
{
[DllImport("libopenGJKlib", EntryPoint="csFunction", CallingConvention = CallingConvention.StdCall)]
static extern double gjk(int na, double [,] ia, int nb, double [,] ib);
public static void Main(string[] args)
{
double dist;
// Define array A with coordinates
int nCoordsA = 9;
var inCoordsA = new double[3,9] { {0.0 , 2.3 , 8.1 , 4.3 ,2.5 , 7.1 , 1.0 , 3.3 , 6.0} , { 5.5 , 1.0 , 4.0 , 5.0 ,1.0, 1.0, 1.5, 0.5 , 1.4} ,{ 0.0 , -2.0, 2.4, 2.2, 2.3 , 2.4 , 0.3 , 0.3 , 0.2} };
// Define array B with coordinates
int nCoordsB = 9;
var inCoordsB = new double[3,9] { {-0.0 , -2.3 , -8.1 , -4.3 ,-2.5 , -7.1 , -1.0 , -3.3 , -6.0} , { -5.5 , -1.0 ,- 4.0 ,- 5.0 ,-1.0, -1.0, -1.5, -0.5 , -1.4} ,{ -0.0 , 2.0, -2.4, -2.2, -2.3 , -2.4 , -0.3 , -0.3 , -0.2} };
// Invoke GJK to compute distance
dist = gjk( nCoordsA, inCoordsA, nCoordsB, inCoordsB );
// Output results
var s = string.Format("{0:0.##}", dist);
var message = string.Format("The distance between {0} is {1}","A and B",s);
Console.WriteLine(message);
Console.WriteLine("Press any key to exit");
Console.ReadLine();
}
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* This file is part of openGJK. *
* *
* openGJK is free software: you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation, either version 3 of the License, or *
* any later version. *
* *
* openGJK is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
* *
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
* Copyright (C) Mattia Montanari 2018 - 2019 *
* http://iel.eng.ox.ac.uk/?page_id=504 *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
using System;
using System.Runtime.InteropServices;
public class Tester
{
[DllImport("libopenGJKlib", EntryPoint="csFunction", CallingConvention = CallingConvention.StdCall)]
static extern double gjk(int na, double [,] ia, int nb, double [,] ib);
public static void Main(string[] args)
{
double dist;
// Define array A with coordinates
int nCoordsA = 9;
var inCoordsA = new double[3,9] { {0.0 , 2.3 , 8.1 , 4.3 ,2.5 , 7.1 , 1.0 , 3.3 , 6.0} , { 5.5 , 1.0 , 4.0 , 5.0 ,1.0, 1.0, 1.5, 0.5 , 1.4} ,{ 0.0 , -2.0, 2.4, 2.2, 2.3 , 2.4 , 0.3 , 0.3 , 0.2} };
// Define array B with coordinates
int nCoordsB = 9;
var inCoordsB = new double[3,9] { {-0.0 , -2.3 , -8.1 , -4.3 ,-2.5 , -7.1 , -1.0 , -3.3 , -6.0} , { -5.5 , -1.0 ,- 4.0 ,- 5.0 ,-1.0, -1.0, -1.5, -0.5 , -1.4} ,{ -0.0 , 2.0, -2.4, -2.2, -2.3 , -2.4 , -0.3 , -0.3 , -0.2} };
// Invoke GJK to compute distance
dist = gjk( nCoordsA, inCoordsA, nCoordsB, inCoordsB );
// Output results
var s = string.Format("{0:0.##}", dist);
var message = string.Format("The distance between {0} is {1}","A and B",s);
Console.WriteLine(message);
Console.WriteLine("Press any key to exit");
Console.ReadLine();
}
}

140
examples/matlab/main.m
View File

@@ -1,78 +1,62 @@
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% ##### # # # %
% #### ##### ###### # # # # # # # %
% # # # # # ## # # # # # %
% # # # # ##### # # # # #### # ### %
% # # ##### # # # # # # # # # # %
% # # # # # ## # # # # # # %
% #### # ###### # # ##### ##### # # %
% %
% This file is part of openGJK. %
% %
% openGJK is free software: you can redistribute it and/or modify %
% it under the terms of the GNU General Public License as published by %
% the Free Software Foundation, either version 3 of the License, or %
% any later version. %
% %
% openGJK is distributed in the hope that it will be useful, %
% but WITHOUT ANY WARRANTY; without even the implied warranty of %
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The %
% GNU General Public License for more details. %
% %
% You should have received a copy of the GNU General Public License %
% along with Foobar. If not, see <https://www.gnu.org/licenses/>. %
% %
% openGJK: open-source Gilbert-Johnson-Keerthi algorithm %
% Copyright (C) Mattia Montanari 2018 - 2019 %
% http://iel.eng.ox.ac.uk/?page_id=504 %
% %
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% %
% This file runs an example to illustrate how to cll the openGJK library %
% withing Matlab. It assumes that a mex file openGJK is availalbe, see %
% the runme.m script for information on how to compile it. %
% The example computes the minimum distance between two polytopes in 3D, %
% A and B, both defined as a list of points. %
% %
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% DEFINE BODY A AS 3xN MATRIX, WHERE N IS THE NUMBER OF VERTICES OF BODY A
A = [ 0.0 2.3 8.1 4.3 2.5 7.1 1.0 3.3 6.0
5.5 1.0 4.0 5.0 1.0 1.0 1.5 0.5 1.4
0.0 -2.0 2.4 2.2 2.3 2.4 0.3 0.3 0.2];
% DEFINE BODY B IN THE OPPOSITE QUADRANT OF BODY A
B = -A;
% COMPUTE MINIMUM DISTANCE AND RETURN VALUE
dist = openGJK( A, B );
fprintf('The minimum distance between A and B is %.2f\n',dist);
% VISUALISE RESULTS
% .. create new figure
figure('units','centimeters', 'WindowStyle','normal', 'color','w',...
'Position',[0 8.5 9 6],'defaultAxesColorOrder',parula,...
'Renderer','opengl')
% .. adjust properties
axis equal tight off; hold all;
% .. display body A
DT = delaunayTriangulation(A');
[K,~] = convexHull(DT);
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
'EdgeColor','none','FaceColor',[.4 1 .9 ],...
'FaceLighting','flat' )
% .. display body B
DT = delaunayTriangulation(B');
[K,~] = convexHull(DT);
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
'EdgeColor','none','FaceColor',[.4 1 .8 ],...
'FaceLighting','flat' )
% .. represent the computed distance as a sphere
[x,y,z] = sphere(100);
surf(x.*dist/2,y.*dist/2,z.*dist/2,'facecolor',[.9 .9 .9],...
'EdgeColor','none','FaceLighting','flat','SpecularColorReflectance',0,...
'SpecularStrength',1,'SpecularExponent',10,'facealpha',.7)
% ... adjust point of view
view(42,21)
% ... add light
light('Position',[5 -10 20],'Style','local');
% _____ _ _ __ %
% / ____| | | |/ / %
% ___ _ __ ___ _ __ | | __ | | ' / %
% / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < %
% | (_) | |_) | __/ | | | |__| | |__| | . \ %
% \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ %
% | | %
% |_| %
% %
% Copyright 2022 Mattia Montanari, University of Oxford %
% %
% This program is free software: you can redistribute it and/or modify it under %
% the terms of the GNU General Public License as published by the Free Software %
% Foundation, either version 3 of the License. You should have received a copy %
% of the GNU General Public License along with this program. If not, visit %
% %
% https://www.gnu.org/licenses/ %
% %
% This program is distributed in the hope that it will be useful, but WITHOUT %
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS %
% FOR A PARTICULAR PURPOSE. See GNU General Public License for details. %
% DEFINE BODY A AS 3xN MATRIX, WHERE N IS THE NUMBER OF VERTICES OF BODY A
A = [ 0.0 2.3 8.1 4.3 2.5 7.1 1.0 3.3 6.0
5.5 1.0 4.0 5.0 1.0 1.0 1.5 0.5 1.4
0.0 -2.0 2.4 2.2 2.3 2.4 0.3 0.3 0.2];
% DEFINE BODY B IN THE OPPOSITE QUADRANT OF BODY A
B = -A;
% COMPUTE MINIMUM DISTANCE AND RETURN VALUE
dist = openGJK( A, B );
fprintf('The minimum distance between A and B is %.2f\n',dist);
% VISUALISE RESULTS
% .. create new figure
figure('units','centimeters', 'WindowStyle','normal', 'color','w',...
'Position',[0 8.5 9 6],'defaultAxesColorOrder',parula,...
'Renderer','opengl')
% .. adjust properties
axis equal tight off; hold all;
% .. display body A
DT = delaunayTriangulation(A');
[K,~] = convexHull(DT);
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
'EdgeColor','none','FaceColor',[.4 1 .9 ],...
'FaceLighting','flat' )
% .. display body B
DT = delaunayTriangulation(B');
[K,~] = convexHull(DT);
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
'EdgeColor','none','FaceColor',[.4 1 .8 ],...
'FaceLighting','flat' )
% .. represent the computed distance as a sphere
[x,y,z] = sphere(100);
surf(x.*dist/2,y.*dist/2,z.*dist/2,'facecolor',[.9 .9 .9],...
'EdgeColor','none','FaceLighting','flat','SpecularColorReflectance',0,...
'SpecularStrength',1,'SpecularExponent',10,'facealpha',.7)
% ... adjust point of view
view(42,21)
% ... add light
light('Position',[5 -10 20],'Style','local');

142
examples/matlab/runme.m
View File

@@ -1,79 +1,65 @@
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% ##### # # # %
% #### ##### ###### # # # # # # # %
% # # # # # ## # # # # # %
% # # # # ##### # # # # #### # ### %
% # # ##### # # # # # # # # # # %
% # # # # # ## # # # # # # %
% #### # ###### # # ##### ##### # # %
% %
% This file is part of openGJK. %
% %
% openGJK is free software: you can redistribute it and/or modify %
% it under the terms of the GNU General Public License as published by %
% the Free Software Foundation, either version 3 of the License, or %
% any later version. %
% %
% openGJK is distributed in the hope that it will be useful, %
% but WITHOUT ANY WARRANTY; without even the implied warranty of %
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The %
% GNU General Public License for more details. %
% %
% You should have received a copy of the GNU General Public License %
% along with Foobar. If not, see <https://www.gnu.org/licenses/>. %
% %
% openGJK: open-source Gilbert-Johnson-Keerthi algorithm %
% Copyright (C) Mattia Montanari 2018 - 2019 %
% http://iel.eng.ox.ac.uk/?page_id=504 %
% %
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% %
% This file compiles a mex function from the openGJK library and runs an %
% example. If the mex function cannot be compiled an error is returned. %
% %
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% CLEAR ALL VARIABLES
clearvars
% SELECT OPTIMISATION FLAG - FASTER BUT NOT SUITABLE FOR DEBUGGING
if 0
optflug = '-g'; %#ok<*UNRCH>
else
optflug = '-O';
end
% SELECT SILET COMPILATION MODE.
if 1
silflag = '-silent';
else
silflag = '-v';
end
% TRY COMPILING MEX FILE
fprintf('Compiling mex function... ')
try
mex(fullfile('..','..','src','openGJK.c'),... % Source of openGJK
'-largeArrayDims', ... % Support large arrays
optflug, ... % Compiler flag for debug/optimisation
fullfile('-I..','..','include'),... % Folder to header files
'-outdir', pwd,... % Ouput directory for writing mex function
'-output', 'openGJK',... % Name of ouput mex file
'-DMATLABDOESMEXSTUFF',... % Define variable for mex function in source files
silflag ) % Silent/verbose flag
% File compiled without errors. Return path and name of mex file
fprintf('completed!\n')
fprintf('The following mex file has been generated:')
fprintf('\t%s\n',[pwd,filesep,'openGJK.',mexext])
catch
% Build failed, refer to documentation
fprintf('\n\n ERROR DETECTED! Mex file cannot be compiled.\n')
fprintf('\tFor more information, see ')
fprintf('<a href="http://www.mathworks.com/help/matlab/ref/mex.html">this documentation page</a>.\n\n')
return
end
% RUN EXAMPLE
fprintf('Running example... ')
main
% _____ _ _ __ %
% / ____| | | |/ / %
% ___ _ __ ___ _ __ | | __ | | ' / %
% / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < %
% | (_) | |_) | __/ | | | |__| | |__| | . \ %
% \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ %
% | | %
% |_| %
% %
% Copyright 2022 Mattia Montanari, University of Oxford %
% %
% This program is free software: you can redistribute it and/or modify it under %
% the terms of the GNU General Public License as published by the Free Software %
% Foundation, either version 3 of the License. You should have received a copy %
% of the GNU General Public License along with this program. If not, visit %
% %
% https://www.gnu.org/licenses/ %
% %
% This program is distributed in the hope that it will be useful, but WITHOUT %
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS %
% FOR A PARTICULAR PURPOSE. See GNU General Public License for details. %
clearvars
% SELECT OPTIMISATION FLAG - FASTER BUT NOT SUITABLE FOR DEBUGGING
if 0
optflug = '-g'; %#ok<*UNRCH>
else
optflug = '-O';
end
% SELECT SILET COMPILATION MODE.
if 1
silflag = '-silent';
else
silflag = '-v';
end
% TRY COMPILING MEX FILE
fprintf('Compiling mex function... ')
try
mex(fullfile('..','..','src','openGJK.c'),... % Source of openGJK
'-largeArrayDims', ... % Support large arrays
optflug, ... % Compiler flag for debug/optimisation
fullfile('-I..','..','include'),... % Folder to header files
'-outdir', pwd,... % Ouput directory for writing mex function
'-output', 'openGJK',... % Name of ouput mex file
'-DMATLABDOESMEXSTUFF',... % Define variable for mex function in source files
silflag ) % Silent/verbose flag
% File compiled without errors. Return path and name of mex file
fprintf('completed!\n')
fprintf('The following mex file has been generated:')
fprintf('\t%s\n',[pwd,filesep,'openGJK.',mexext])
catch
% Build failed, refer to documentation
fprintf('\n\n ERROR DETECTED! Mex file cannot be compiled.\n')
fprintf('\tFor more information, see ')
fprintf('<a href="http://www.mathworks.com/help/matlab/ref/mex.html">this documentation page</a>.\n\n')
return
end
% RUN EXAMPLE
fprintf('Running example... ')
main
fprintf('completed!\n')

2
examples/python/build.sh
View File

@@ -1,2 +1,2 @@
#!/bin/bash
#!/bin/bash
g++ -Wall -fPIC -fopenmp -shared `python3 -m pybind11 --includes` -I ../../include -I/usr/include/eigen3 pyopenGJK.cpp ../../src/openGJK.c -o opengjkc`python3-config --extension-suffix`

11
examples/python/pyopenGJK.cpp
View File

@@ -3,23 +3,22 @@
#include <pybind11/pybind11.h>
namespace py = pybind11;
PYBIND11_MODULE(opengjkc, m)
{
PYBIND11_MODULE(opengjkc, m) {
m.def("gjk",
[](Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor>& arr1,
Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor>& arr2)
[](Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor> &arr1,
Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor> &arr2)
-> double {
struct simplex s;
struct bd bd1;
struct bd bd2;
bd1.numpoints = arr1.rows();
std::vector<double*> arr1_rows(arr1.rows());
std::vector<double *> arr1_rows(arr1.rows());
for (int i = 0; i < arr1.rows(); ++i)
arr1_rows[i] = arr1.row(i).data();
bd1.coord = arr1_rows.data();
bd2.numpoints = arr2.rows();
std::vector<double*> arr2_rows(arr2.rows());
std::vector<double *> arr2_rows(arr2.rows());
for (int i = 0; i < arr2.rows(); ++i)
arr2_rows[i] = arr2.row(i).data();
bd2.coord = arr2_rows.data();

370
examples/python/test.py
View File

@@ -1,185 +1,185 @@
import opengjkc as opengjk
from scipy.spatial.transform import Rotation as R
import numpy as np
import pytest
#from IPython import embed
def settol():
return 1e-12
def distance_point_to_line_3D(P1, P2, point):
"""
distance from point to line
"""
return np.linalg.norm(np.cross(P2-P1, P1-point))/np.linalg.norm(P2-P1)
def distance_point_to_plane_3D(P1, P2, P3, point):
"""
Distance from point to plane
"""
return np.abs(np.dot(np.cross(P2-P1, P3-P1) /
np.linalg.norm(np.cross(P2-P1, P3-P1)), point-P2))
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0, -2])
def test_line_point_distance(delta):
line = np.array([[0.1, 0.2, 0.3], [0.5, 0.8, 0.7]], dtype=np.float64)
point_on_line = line[0] + 0.27*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
distance = opengjk.gjk(line, point)
actual_distance = distance_point_to_line_3D(
line[0], line[1], point)
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0])
def test_line_line_distance(delta):
line = np.array([[-0.5, -0.7, -0.3], [1, 2, 3]], dtype=np.float64)
point_on_line = line[0] + 0.38*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
line_2 = np.array([point, [2, 5, 6]], dtype=np.float64)
distance = opengjk.gjk(line, line_2)
actual_distance = distance_point_to_line_3D(
line[0], line[1], line_2[0])
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1**(3*i) for i in range(6)])
def test_tri_distance(delta):
tri_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
tri_2 = np.array([[1, delta, 0], [3, 1.2, 0], [
1, 1, 0]], dtype=np.float64)
P1 = tri_1[2]
P2 = tri_1[1]
point = tri_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.gjk(tri_1, tri_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
#embed()
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1*0.1**(3*i) for i in range(6)])
def test_quad_distance2d(delta):
quad_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[1, 1, 0]], dtype=np.float64)
quad_2 = np.array([[0, 1+delta, 0], [2, 2, 0],
[2, 4, 0], [4, 4, 0]], dtype=np.float64)
P1 = quad_1[2]
P2 = quad_1[3]
point = quad_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.gjk(quad_1, quad_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [1*0.5**(3*i) for i in range(7)])
def test_tetra_distance_3d(delta):
tetra_1 = np.array([[0, 0, 0.2], [1, 0, 0.1], [0, 1, 0.3],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.gjk(tetra_1, tetra_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [(-1)**i*np.sqrt(2)*0.1**(3*i)
for i in range(6)])
def test_tetra_collision_3d(delta):
tetra_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.gjk(tetra_1, tetra_2)
if delta < 0:
assert(np.isclose(distance, 0, atol=settol()))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol()))
@pytest.mark.parametrize("delta", [0, -0.1, -0.49, -0.51])
def test_hex_collision_3d(delta):
hex_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0],
[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]],
dtype=np.float64)
P0 = np.array([1.5+delta, 1.5+delta, 0.5], dtype=np.float64)
P1 = np.array([2, 2, 1], dtype=np.float64)
P2 = np.array([2, 1.25, 0.25], dtype=np.float64)
P3 = P1 + P2 - P0
quad_1 = np.array([P0, P1, P2, P3], dtype=np.float64)
n = (np.cross(quad_1[1]-quad_1[0], quad_1[2]-quad_1[0]) /
np.linalg.norm(
np.cross(quad_1[1]-quad_1[0],
quad_1[2]-quad_1[0])))
quad_2 = quad_1 + n
hex_2 = np.zeros((8, 3), dtype=np.float64)
hex_2[:4, :] = quad_1
hex_2[4:, :] = quad_2
actual_distance = np.linalg.norm(
np.array([1, 1, P0[2]], dtype=np.float64)-hex_2[0])
distance = opengjk.gjk(hex_1, hex_2)
if P0[0] < 1:
assert(np.isclose(distance, 0, atol=settol()))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol()))
@pytest.mark.parametrize("c0", [0, 1, 2, 3])
@pytest.mark.parametrize("c1", [0, 1, 2, 3])
def test_cube_distance(c0, c1):
cubes = [np.array([[-1, -1, -1], [1, -1, -1], [-1, 1, -1], [1, 1, -1],
[-1, -1, 1], [1, -1, 1], [-1, 1, 1], [1, 1, 1]],
dtype=np.float64)]
r = R.from_euler('z', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
r = R.from_euler('y', np.arctan2(1.0, np.sqrt(2)))
cubes.append(r.apply(cubes[1]))
r = R.from_euler('y', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
dx = cubes[c0][:,0].max() - cubes[c1][:,0].min()
cube0 = cubes[c0]
for delta in [1e8, 1.0, 1e-4, 1e-8, 1e-12]:
cube1 = cubes[c1] + np.array([dx + delta, 0, 0])
distance = opengjk.gjk(cube0, cube1)
print(distance, delta)
assert(np.isclose(distance, delta))
def test_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = np.random.rand(i, 3)
arr2 = np.random.rand(j, 3)
opengjk.gjk(arr1, arr2)
def test_large_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = 10000.0*np.random.rand(i, 3)
arr2 = 10000.0*np.random.rand(j, 3)
opengjk.gjk(arr1, arr2)
import opengjkc as opengjk
from scipy.spatial.transform import Rotation as R
import numpy as np
import pytest
#from IPython import embed
def settol():
return 1e-12
def distance_point_to_line_3D(P1, P2, point):
"""
distance from point to line
"""
return np.linalg.norm(np.cross(P2-P1, P1-point))/np.linalg.norm(P2-P1)
def distance_point_to_plane_3D(P1, P2, P3, point):
"""
Distance from point to plane
"""
return np.abs(np.dot(np.cross(P2-P1, P3-P1) /
np.linalg.norm(np.cross(P2-P1, P3-P1)), point-P2))
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0, -2])
def test_line_point_distance(delta):
line = np.array([[0.1, 0.2, 0.3], [0.5, 0.8, 0.7]], dtype=np.float64)
point_on_line = line[0] + 0.27*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
distance = opengjk.gjk(line, point)
actual_distance = distance_point_to_line_3D(
line[0], line[1], point)
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0])
def test_line_line_distance(delta):
line = np.array([[-0.5, -0.7, -0.3], [1, 2, 3]], dtype=np.float64)
point_on_line = line[0] + 0.38*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
line_2 = np.array([point, [2, 5, 6]], dtype=np.float64)
distance = opengjk.gjk(line, line_2)
actual_distance = distance_point_to_line_3D(
line[0], line[1], line_2[0])
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1**(3*i) for i in range(6)])
def test_tri_distance(delta):
tri_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
tri_2 = np.array([[1, delta, 0], [3, 1.2, 0], [
1, 1, 0]], dtype=np.float64)
P1 = tri_1[2]
P2 = tri_1[1]
point = tri_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.gjk(tri_1, tri_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
#embed()
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [0.1*0.1**(3*i) for i in range(6)])
def test_quad_distance2d(delta):
quad_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[1, 1, 0]], dtype=np.float64)
quad_2 = np.array([[0, 1+delta, 0], [2, 2, 0],
[2, 4, 0], [4, 4, 0]], dtype=np.float64)
P1 = quad_1[2]
P2 = quad_1[3]
point = quad_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.gjk(quad_1, quad_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [1*0.5**(3*i) for i in range(7)])
def test_tetra_distance_3d(delta):
tetra_1 = np.array([[0, 0, 0.2], [1, 0, 0.1], [0, 1, 0.3],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.gjk(tetra_1, tetra_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
@pytest.mark.parametrize("delta", [(-1)**i*np.sqrt(2)*0.1**(3*i)
for i in range(6)])
def test_tetra_collision_3d(delta):
tetra_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.gjk(tetra_1, tetra_2)
if delta < 0:
assert(np.isclose(distance, 0, atol=settol()))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol()))
@pytest.mark.parametrize("delta", [0, -0.1, -0.49, -0.51])
def test_hex_collision_3d(delta):
hex_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0],
[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]],
dtype=np.float64)
P0 = np.array([1.5+delta, 1.5+delta, 0.5], dtype=np.float64)
P1 = np.array([2, 2, 1], dtype=np.float64)
P2 = np.array([2, 1.25, 0.25], dtype=np.float64)
P3 = P1 + P2 - P0
quad_1 = np.array([P0, P1, P2, P3], dtype=np.float64)
n = (np.cross(quad_1[1]-quad_1[0], quad_1[2]-quad_1[0]) /
np.linalg.norm(
np.cross(quad_1[1]-quad_1[0],
quad_1[2]-quad_1[0])))
quad_2 = quad_1 + n
hex_2 = np.zeros((8, 3), dtype=np.float64)
hex_2[:4, :] = quad_1
hex_2[4:, :] = quad_2
actual_distance = np.linalg.norm(
np.array([1, 1, P0[2]], dtype=np.float64)-hex_2[0])
distance = opengjk.gjk(hex_1, hex_2)
if P0[0] < 1:
assert(np.isclose(distance, 0, atol=settol()))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol()))
@pytest.mark.parametrize("c0", [0, 1, 2, 3])
@pytest.mark.parametrize("c1", [0, 1, 2, 3])
def test_cube_distance(c0, c1):
cubes = [np.array([[-1, -1, -1], [1, -1, -1], [-1, 1, -1], [1, 1, -1],
[-1, -1, 1], [1, -1, 1], [-1, 1, 1], [1, 1, 1]],
dtype=np.float64)]
r = R.from_euler('z', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
r = R.from_euler('y', np.arctan2(1.0, np.sqrt(2)))
cubes.append(r.apply(cubes[1]))
r = R.from_euler('y', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
dx = cubes[c0][:,0].max() - cubes[c1][:,0].min()
cube0 = cubes[c0]
for delta in [1e8, 1.0, 1e-4, 1e-8, 1e-12]:
cube1 = cubes[c1] + np.array([dx + delta, 0, 0])
distance = opengjk.gjk(cube0, cube1)
print(distance, delta)
assert(np.isclose(distance, delta))
def test_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = np.random.rand(i, 3)
arr2 = np.random.rand(j, 3)
opengjk.gjk(arr1, arr2)
def test_large_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = 10000.0*np.random.rand(i, 3)
arr2 = 10000.0*np.random.rand(j, 3)
opengjk.gjk(arr1, arr2)