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

New sub-algorithm

Browse Source
This commit is contained in:
Mattia Montanari
2020-04-15 23:18:36 +01:00
parent 095b9f49c9
commit 214b2c47d6
24 changed files with 3173 additions and 12010 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
examples/c/CMakeLists.txt Normal file
View File

@@ -0,0 +1,78 @@
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
# ##### # # # #
# #### ##### ###### # # # # # # # #
# # # # # # ## # # # # # #
# # # # # ##### # # # # #### # ### #
# # # ##### # # # # # # # # # # #
# # # # # # ## # # # # # # #
# #### # ###### # # ##### ##### # # #
# #
# 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 after build
add_custom_command(
TARGET demo POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy
${CMAKE_SOURCE_DIR}/examples/c/userP.dat
${CMAKE_CURRENT_BINARY_DIR}/userP.dat
COMMAND ${CMAKE_COMMAND} -E copy
${CMAKE_SOURCE_DIR}/examples/c/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 ()
include(AddCMockaTest)
find_library(LIB_CMOCKA cmocka)
target_link_libraries(demo ${CMOCKA_LIBRARY})
if(NOT LIB_CMOCKA)
message(STATUS "${LIB_CMOCKA} library not found ")
endif()
if (UNIT_TESTING)
target_link_libraries(demo cmocka)
add_test( ${TEST_NAME} demo)
endif (UNIT_TESTING)
message(STATUS "Completed CMake setting for ${PROJECT_NAME}" )

178
examples/c/main.c Normal file
View File

@@ -0,0 +1,178 @@
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* 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_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;
}
/* 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);
}

10
examples/c/userP.dat Normal file
View File

@@ -0,0 +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

10
examples/c/userQ.dat Normal file
View File

@@ -0,0 +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

67
examples/cs/main.cs Normal file
View File

@@ -0,0 +1,67 @@
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* 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
{
#if UNIX
[DllImport("libopenGJKlib.so", EntryPoint="csFunction", CallingConvention = CallingConvention.StdCall)]
#else
[DllImport("openGJKlib", EntryPoint = "csFunction", CallingConvention = CallingConvention.StdCall)]
#endif
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();
}
}

78
examples/matlab/main.m Normal file
View File

@@ -0,0 +1,78 @@
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% ##### # # # %
% #### ##### ###### # # # # # # # %
% # # # # # ## # # # # # %
% # # # # ##### # # # # #### # ### %
% # # ##### # # # # # # # # # # %
% # # # # # ## # # # # # # %
% #### # ###### # # ##### ##### # # %
% %
% 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');

79
examples/matlab/runme.m Normal file
View File

@@ -0,0 +1,79 @@
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% ##### # # # %
% #### ##### ###### # # # # # # # %
% # # # # # ## # # # # # %
% # # # # ##### # # # # #### # ### %
% # # ##### # # # # # # # # # # %
% # # # # # ## # # # # # # %
% #### # ###### # # ##### ##### # # %
% %
% 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('..','..','lib','src','openGJK.c'),... % Source of openGJK
'-largeArrayDims', ... % Support large arrays
optflug, ... % Compiler flag for debug/optimisation
fullfile('-I..','..','lib','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')