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

New sub-algorithm

fixes-turtlebasket
Mattia Montanari 2020-04-15 23:18:36 +01:00
parent 095b9f49c9
commit 214b2c47d6
24 changed files with 3173 additions and 12010 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
CMakeLists.txt
View File

@ -20,7 +20,7 @@
# 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/>. #
# along with openGJK. If not, see <https://www.gnu.org/licenses/>. #
# #
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
# Copyright (C) Mattia Montanari 2018 - 2019 #
@ -28,67 +28,57 @@
# #
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
cmake_minimum_required(VERSION 3.5)
project(openGJK)
message( "[${PROJECT_NAME}] CMake setting ..")
set(CMAKE_C_STANDARD 11)
message(STATUS "Version : " ${openGJK_VERSION} )
message(STATUS "Build type : " ${CMAKE_BUILD_TYPE} )
message("[${CMAKE_PROJECT_NAME}] Welcome, please change user options if needed.")
# Select source files
set( SOURCE_FILES src/openGJK.c )
set( SOURCE_HEADS include/openGJK/openGJK.h)
# APPLY DEFAULT SETTINGS
if(NOT CMAKE_BUILD_TYPE)
message("[${CMAKE_PROJECT_NAME}] Use default CMAKE_BUILD_TYPE")
set(CMAKE_BUILD_TYPE Release)
endif()
IF(USE_PREDICATES)
# for adpative floating-point artim.
set( SOURCE_FILES ${SOURCE_FILES} ext/predicates.c)
set( SOURCE_HEADS ${SOURCE_HEADS} ext/predicates.h)
# Add flag for adpative floating-point artim.
add_definitions(-DADAPTIVEFP)
ENDIF()
IF(BUILD_STATIC_LIB)
message(STATUS "Library type: " Static )
add_library(${PROJECT_NAME} STATIC ${SOURCE_FILES} ${SOURCE_HEADS})
add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=FALSE)
ELSE()
message(STATUS "Library type: " Shared )
add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${SOURCE_HEADS})
add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=FALSE)
ENDIF(BUILD_STATIC_LIB)
# PLATFORM-SPECIFIC SETTING
if (UNIX)
find_library(M_LIB m)
set(CMAKE_C_FLAGS "-lm")
set(CMAKE_CXX_FLAGS "-lm")
else ()
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
endif ()
# COMPILER SETTING
IF(CMAKE_BUILD_TYPE MATCHES Release)
set(CMAKE_BUILD_TYPE Release)
ELSEIF(CMAKE_BUILD_TYPE MATCHES Debug)
set(CMAKE_BUILD_TYPE Debug)
find_package(OpenMP)
if (OPENMP_FOUND)
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
endif()
# ADD DEFAULT COMPILER FLAGS
include(CompilerFlags)
# Link include file
target_include_directories( ${PROJECT_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
target_link_libraries(${PROJECT_NAME} ${CMOCKA_LIBRARY} OpenMP::OpenMP_C )
IF(USE_PREDICATES)
# for adpative floating-point artim.
target_include_directories( ${PROJECT_NAME}
PUBLIC ${PROJECT_SOURCE_DIR}/ext
)
ENDIF()
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
# using GCC
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra -finline-functions")
set(CMAKE_CXX_FLAGS_DEBUG "-g -DDEBUG")
set(CMAKE_CXX_FLAGS_RELEASE "-O3")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -finline-functions")
set(CMAKE_C_FLAGS_DEBUG "-g -DDEBUG")
set(CMAKE_C_FLAGS_RELEASE "-O3")
add_compile_options(-static-libgcc -static-libstdc++ )
add_definitions(-DMT)
elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
# using Visual Studio C++
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4131 /wd4701 /wd4255 /wd4710 /wd4820 /wd4711 /wd5045")
set(CMAKE_CXX_FLAGS_DEBUG "-DDEBUG /D_DEBUG /MDd /Zi /Ob0 /Od /RTC1")
set(CMAKE_CXX_FLAGS_RELEASE "/Ox")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /wd4131 /wd4701 /wd4255 /wd4710 /wd4820 /wd4711 /wd5045")
set(CMAKE_C_FLAGS_DEBUG "-DDEBUG /D_DEBUG /MDd /Zi /Ob0 /Od /RTC1")
set(CMAKE_C_FLAGS_RELEASE "/Ox")
set(CMAKE_SUPPRESS_REGENERATION true)
endif()
# DEBUG FLAGS
IF(CMAKE_BUILD_TYPE MATCHES Debug)
add_definitions(-DDEBUG)
ENDIF()
# INCLUDE LIBRARY AND EXAMPLE DIR
add_subdirectory(lib)
add_subdirectory(example1_c)

193
README.md
View File

@ -2,154 +2,30 @@
openGJK {#mainpage}
=======
The openGJK library uses the Gilbert-Johnson-Keerthi (GJK) algorithm to
compute the minimum distance between convex polytopes. The
implementation follows the description presented in
"[Improving the GJK Algorithm for Faster and More Reliable Distance
Queries Between Convex Objects. ACM Trans. on Graph. 2017](https://dl.acm.org/citation.cfm?id=3083724)" and has been tested
on Unix and Windows systems for C, C# and Matlab programs.
OpenGJK implements a new version of the Gilbert-Johnson-Keerthi (GJK) algorithm to
compute the minimum distance between convex polytopes. OpenGJK is a C library which was tested on Unix and Windows using different compilers for multi-threaded applications.
This library offers researchers a tool that works
out of the box: you can import it in your program and use it to measure
the distance between two convex polytopes in 3D. All it needs are the
coordinates of the vertices describing the two bodies.
This library is not optimised for production, but it does provide a comprehensive and robust implementation. It is sufficiently fast for
most applications, and you can also build from here to suite your own
application. For instance, openGJK is not for incremental and is not
for NURBS, but it offers a good starting point for such specific
applications.
## Getting Started
Using openGJK is very simple. This guide will help you getting
started compiling and using openGJK.
### When should I use openGJK?
OpenGJK is designed with accuracy and robustness in mind and is
suitable for engineering simulations. Good use of this library
include the finite element method (FEM) and discrete element method (DEM).
Basically, openGJK can measure the distance between **any convex polytope**. For example:
- line segments
- triangles
- tetrahedrons
- cubes.
### Installing the openGJK library
#### Prerequisites
1. A compiler (gnu or Microsoft Visual Studio for C)
2. CMake version 3.5 or above
3. Only for the Matlab interface you will need to build mex files (find out the requisites from [Mathworks documentation](https://uk.mathworks.com/help/matlab/matlab_external/what-you-need-to-build-mex-files.html)).
4. Only for the C# interface on Unix you will need [mono](http://www.mono-project.com/) and Microsoft Visual Studio toolchain for C# on Windows.
### Installation
There are CMake files for compiling openGJK in the usual
way:
1. Create a new folder in the folder containing this readme file.
2. Move into that folder and type `cmake -G <duild-system> ..`. For example,
on Windows you can type `cmake -G "Visual Studio 15 2017 Win64" ..`, on Unix `cmake -G "Unix Makefiles" ..`.
3. Use the files generated by Cmake to build the library. Whether you compile
with `make` or an IDE, you will build a shared library and an executable
for the C example. For Matlab and C# applications, see sections below.
To install the library you should copy the header file openGJK.h and the binaries in a folder accessible in the search path by all users (on Unix this would normally be /usr/local).
### Automated documentation
The folder `doc` contains a Doxygen file for generating the documentation of the whole
library. To build the documentation cd into `doc` and call Doxygen from the command line simply by typing `doxygen`. If correctly installed, Doxygen will create html documentation with graphs illustrating the call stack of the functions of the library.
## API user reference
```double gjk( struct bodyA, struct bodyB, struct simplex)```
### Parameters
* **bodyA** The first body.
* **bodyB** The second body.
* **simplex** The simplex used the GJK algorithm at the first iteration.
### Returns
* **double** the minimum distance between bodyA and bodyB.
### Description
The function `gjk` computes the minimum Euclidean distance between two bodies using the
GJK algorithm. Note that the simplex used at the first iteration may be initialised by the user, but this is not necessary.
Detailed information about the algorithm see "[Improving the GJK Algorithm for Faster and More Reliable Distance
Queries Between Convex Objects. ACM Trans. on Graph. 2017](https://dl.acm.org/citation.cfm?id=3083724)".
## Configuration
When should I use openGJK?
--------------------------
openGJK comes in two flavours: *accurate* and *fast* (default). You can
change before compiling by editing the main 'lib\CMakeLists.txt' file
(in the folder `lib`). Set the option `VERSION_ACCURATE` to `ON` and
run CMake. You can verify what version is being compiled from the terminal,
if you do not see "Version: Accurate" when calling CMake, you have to clean
the CMake cache.
OpenGJK is designed with speed, accuracy and robustness in mind and is therefore suitable for engineering, robotics and computer graphics simulations.
Basically, openGJK can be used in any application where the distance between **any convex polytope** is required.
## Examples
Compile and run
---------------
This section presents three examples on how to use openGJK with C, C# and Matlab.
All the examples have been tested both Linux and Windows; the former used `make` and `gcc`,
the latter using `Visual studio 2017` and its compiler. Only x64 systems have been tested.
To compile the OpenGJK library create a build dir,
and in the build dir call 'cmake ..' followed by 'make'. More details can be found in the INSTALL file.
### C
This example illustrates how to include openGJK in an existing C
program.
There are examples for C, C# and Matlab in the `examples` folder. The INSTALL file provides information on how to run the examples.
All files for the example are in the `example1_c` folder. The executable built with
CMake reads the coordinates of two polytopes from the input files,
respectively userP.dat and userQ.dat, and computes the minimum distance
between them.
Repository content
------------------
Notice that the input files must be in the folder from which the executable
is launched, otherwise an error is returned.
You can edit the coordinates in the input file to test different
polytopes; just remember to edit also the first number in the files
that corresponds to the numbers of vertices that the program will read.
### Matlab
This example illustrates how to invoke openGJK as a regular built-in
Matlab function.
Open Matlab and cd into the `example2_mex` folder. By running the
script `runme.m`, Matlab will first compile a mex file (telling you
about the name of the mex file generated) and will call the script
`main.m`. This invokes openGJK within Matlab and illustrates the
result.
The mex file may be copied and called from any other Matlab project.
### C# #
This example illustrates how to invoke openGJK in an applications written in C#.
The only file required is in the `example3_csharp` folder. This can be compiled in Unix
with mono, or in Windows using Visual Studio. Notice that, however, the openGJK library
is compiled for a specific architecture (usually x64), and this breaks the portability
of the .NET application compiled in this example.
Below are the steps for compiling the C# application on Windows and Linux. Both
procedures assume the dynamic library of openGJK has been already compiled.
#### Compile on Windows
1. Move into the folder `example3_csharp` and create a new folder `example3`.
2. Copy into this folder the openGJK library or make it available in any directory.
3. Open Visual Studio and create a new project. As project type select **Console App (.NET Framework)**.
4. Add to this project the `main.cs` file
5. Set x64 as the target platform, compile the application and run it.
#### Compile on Linux
1. Move into the folder `example3_csharp` and create a new folder `example3`.
2. Copy into this folder the openGJK library or install is so that is available in any directory.
3. Move into that new folder and open a terminal.
4. Type `mcs -out:example3demo -d:UNIX ../main.cs`
5. Run the example by typing `mono example3demo`
## Repository content
This repository contains the following files and folders:
```
@ -191,7 +67,11 @@ This repository contains the following files and folders:
openGJK.c
```
## Where to go next?
More information
----------------
[OpenGJK](http://iel.eng.ox.ac.uk/?page_id=504) was developed at the Impact Engineering Laboratory, University of Oxford.
A clear presentation of the GJK algorithm can be found in the
book by **Van der Bergen** *Collision Detection in Interactive 3D
@ -200,31 +80,16 @@ A clear presentation of the GJK algorithm can be found in the
More details about the GJK algorithm can be found in the original paper
from Gilbert, Johnson and Keerthi [A fast procedure for computing the distance between complex objects in three-dimensional space](http://ieeexplore.ieee.org/document/2083/?arnumber=2083).
OpenGJK implements the GJK algorithm as described in: [Improving the GJK Algorithm for Faster and More Reliable Distance
Queries Between Convex Objects. ACM Trans. on Graph. 2017 ](https://dl.acm.org/citation.cfm?id=3083724). Refer to this papar for further details on the method.
How to cite openGJK
-------------------
If you use openGJK for your research please cite [OpenGJK for C, C# and Matlab: Reliable solutions to distance queries between convex bodies in three-dimensional space. SoftwareX. 2018](https://www.sciencedirect.com/science/article/pii/S2352711018300591).
## Licence
License
-------
This open-source edition of openGJK is released under the terms of
[CC BY-NC-SA 4.0](https://creativecommons.org/licenses/by-nc-sa/4.0/) License.
This means that any software created with this library you must comply
with the terms of this licence. If you are seeking another licence please
contact the author at the address at the end of this file.
openGJK may use the geometric predicates from *Routines for Arbitrary
Precision Floating-point Arithmetic*, by Jonathan Richard Shewchuk,
whose source code is included in the file predicates.c of this
repository for convenience.
------------------------------------------------------------------------
openGJK, Copyright (c) 2018
Impact Engineering Laboratory
Department of Engineering Science
University of Oxford
Parks Road, Oxford, OX1 3PJ
mattia.montanari@eng.ox.ac.uk
------------------------------------------------------------------------
This project is licensed undert the GNU General Public License v3.0.
openGJK: open-source Gilbert-Johnson-Keerthi algorithm
Copyright (C) Mattia Montanari 2018 - 2019
http://iel.eng.ox.ac.uk/?page_id=504

2491
doc/Doxyfile
View File

File diff suppressed because it is too large Load Diff

48
doc/openGJKcustomfooter.html
View File

@ -1,48 +0,0 @@
<!-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* 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 *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -->
<!--BEGIN GENERATE_TREEVIEW-->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
<ul>
$navpath
<li class="footer">$generatedby
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="$relpath^doxygen.png" alt="doxygen"/></a> $doxygenversion </li>
</ul>
</div>
<!--END GENERATE_TREEVIEW-->
<!--BEGIN !GENERATE_TREEVIEW-->
<hr class="footer"/><address class="footer"><small>
$generatedby &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="$relpath^doxygen.png" alt="doxygen"/>
</a> $doxygenversion
</small></address>
<!--END !GENERATE_TREEVIEW-->
</body>
</html>

64
doc/openGJKcustomheader.html
View File

@ -1,64 +0,0 @@
<!-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* 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 *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.14"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>openGJK: Main Page</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="openGJKcustomstyle.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
<tbody>
<tr style="height: 56px;">
<td id="projectlogo"><img alt="Logo" src="oxforduni.jpg" height="100px"/></td>
<td id="projectalign" style="padding-left: 2.5em;">
<div id="projectname">openGJK
&#160;<span id="projectnumber">v 1.0</span>
</div>
<div id="projectbrief">Fast and reliable distance queries in 3D between convex polytopes.</div>
</td>
</tr>
</tbody>
</table>
</div>
<!-- end header part -->

1626
doc/openGJKcustomstyle.css
View File

File diff suppressed because it is too large Load Diff

BIN
doc/oxfordLogo.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 29 KiB

47
example1_c/CMakeLists.txt → examples/c/CMakeLists.txt
View File

@ -20,7 +20,7 @@
# 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/>. #
# along with openGJK. If not, see <https://www.gnu.org/licenses/>. #
# #
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
# Copyright (C) Mattia Montanari 2018 - 2019 #
@ -28,9 +28,13 @@
# #
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
project(openGJKdemo)
project(openGJKdemo VERSION 1.0.0 LANGUAGES C)
message( "[${CMAKE_PROJECT_NAME}] Compiling the executable ..")
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 )
@ -38,8 +42,37 @@ set(SOURCE_FILES main.c )
# Create the executable
add_executable(demo ${SOURCE_FILES})
# Link to openGJK
target_link_libraries(demo openGJKlib )
# 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)
# Report
message( ".. executable DONE!")
# 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}" )

37
example1_c/main.c → examples/c/main.c
View File

@ -36,7 +36,7 @@
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/**
/**
* @file main.c
* @author Mattia Montanari
* @date April 2018
@ -45,9 +45,10 @@
*/
#define _CRT_HAS_CXX17 0
#include <stdlib.h>
#include <stdio.h>
/* For importing openGJK this is Step 1: include header in subfolder. */
/* For importing openGJK this is Step 1: include header in subfolder. */
#include "openGJK/openGJK.h"
#ifndef WIN32
@ -58,7 +59,7 @@
* @brief Function for reading input file with body's coordinates.
*
*/
int readinput ( const char *inputfile, double ***pts, int * out ) {
int readinput(const char *inputfile, double ***pts, int * out) {
int npoints = 0;
int idx = 0;
FILE *fp;
@ -81,13 +82,13 @@ int readinput ( const char *inputfile, double ***pts, int * out ) {
/* Allocate memory. */
double **arr = (double **)malloc(npoints * sizeof(double *));
for (int i=0; i<npoints; i++)
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 )
if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3)
return 1;
}
@ -121,20 +122,20 @@ int main() {
char inputfileA[40] = "userP.dat",
inputfileB[40] = "userQ.dat";
/* Pointers to vertices' coordinates of body 1 and body 2, respectively. */
double (**vrtx1) = NULL,
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 ))
if (readinput(inputfileA, &vrtx1, &nvrtx1))
return (1);
bd1.coord = vrtx1;
bd1.numpoints = nvrtx1;
/* Import coordinates of object 2. */
if (readinput ( inputfileB, &vrtx2, &nvrtx2 ))
if (readinput(inputfileB, &vrtx2, &nvrtx2))
return (1);
bd2.coord = vrtx2;
bd2.numpoints = nvrtx2;
@ -145,31 +146,31 @@ int main() {
#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]);
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]);
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);
dd = gjk(bd1, bd2, &s);
/* Print distance between objects. */
printf ("Distance between bodies %f\n", dd);
printf("Distance between bodies %f\n", dd);
/* Free memory */
for (int i=0; i<bd1.numpoints; i++)
for (int i = 0; i < bd1.numpoints; i++)
free(bd1.coord[i]);
free(bd1.coord);
for (int i=0; i<bd2.numpoints; i++)
for (int i = 0; i < bd2.numpoints; i++)
free(bd2.coord[i]);
free(bd2.coord);

0
example1_c/userP.dat → examples/c/userP.dat
View File

0
example1_c/userQ.dat → examples/c/userQ.dat
View File

0
example3_csharp/main.cs → examples/cs/main.cs
View File

0
example2_mex/main.m → examples/matlab/main.m
View File

4
example2_mex/runme.m → examples/matlab/runme.m
View File

@ -52,10 +52,10 @@ end
% TRY COMPILING MEX FILE
fprintf('Compiling mex function... ')
try
mex(fullfile('..','lib','src','openGJK.c'),... % Source of openGJK
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
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

52
include/openGJK/openGJK.h Normal file
View File

@ -0,0 +1,52 @@
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* Edward Garemo and Mattia Montanari *
* University of Oxford 2019 *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* *
* This is the header file for the openGJK.c file. It defines the openGJK *
* function and it two important structures: bd and simplex. *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
#ifndef __OPENGJK_H__
#define __OPENGJK_H__
#include <stdio.h>
#include <stdlib.h>
#include "math.h"
/**
* @brief Structure of a body.
*/
struct bd {
int numpoints; /**< Number of points defining the body. */
double s[3]; /**< Support mapping computed last. */
double **coord; /**< Pointer to pointer to the points' coordinates. */
};
/**
* @brief Structure for a simplex.
*/
struct simplex {
int nvrtx; /**< Number of simplex's vertices. */
double vrtx[4][3]; /**< Coordinates of simplex's vertices. */
int wids[4]; /**< Label of the simplex's vertices. */
double lambdas[4]; /**< Barycentric coordiantes for each vertex. */
};
/**
* @brief The GJK algorithm which returns the minimum distance between
* two bodies.
*/
extern double gjk(struct bd, struct bd, struct simplex *);
#endif

80
lib/CMakeLists.txt
View File

@ -1,80 +0,0 @@
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
# ##### # # # #
# #### ##### ###### # # # # # # # #
# # # # # # ## # # # # # #
# # # # # ##### # # # # #### # ### #
# # # ##### # # # # # # # # # # #
# # # # # # ## # # # # # # #
# #### # ###### # # ##### ##### # # #
# #
# 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 #
# #
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
project (openGJKlib)
set(CMAKE_C_STANDARD 11)
# SELECT USER OPTIONS
option(VERSION_ACCURATE "Reduce speed to maximise accuracy (OFF)" OFF )
# APPLY USER OPTIONS
IF(VERSION_ACCURATE)
set(USE_PREDICATES ON)
set(openGJK_VERSION "Accurate")
ELSE()
set(USE_PREDICATES OFF)
set(openGJK_VERSION "Fast")
ENDIF()
# COMPILE
message( "[${CMAKE_PROJECT_NAME}] Compiling ..")
message(STATUS "Version (Accurate,Fast): " ${openGJK_VERSION} )
message(STATUS "Build type (Debug,Release): " ${CMAKE_BUILD_TYPE} )
# Select source files
set( SOURCE_FILES src/openGJK.c )
set( SOURCE_HEADS include/openGJK/openGJK.h)
IF(USE_PREDICATES)
# for adpative floating-point artim.
set( SOURCE_FILES ${SOURCE_FILES} ext/predicates.c)
set( SOURCE_HEADS ${SOURCE_HEADS} ext/predicates.h)
# Add flag for adpative floating-point artim.
add_definitions(-DADAPTIVEFP)
ENDIF()
# Create the (dynamic) library
add_library(${PROJECT_NAME} STATIC ${SOURCE_FILES} ${SOURCE_HEADS})
add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=FALSE)
# Link include file
target_include_directories( ${PROJECT_NAME} PUBLIC ${PROJECT_SOURCE_DIR}/include)
IF(USE_PREDICATES)
# for adpative floating-point artim.
target_include_directories( ${PROJECT_NAME}
PUBLIC ${PROJECT_SOURCE_DIR}/ext
)
ENDIF()
# Report
message( ".. DONE!")

4271
lib/ext/predicates.c
View File

File diff suppressed because it is too large Load Diff

51
lib/ext/predicates.h
View File

@ -1,51 +0,0 @@
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* 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 *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
#include <stdio.h>
#include <stdlib.h>
#ifndef CGJK_PREDICATES_H
#define CGJK_PREDICATES_H
#endif //CGJK_PREDICATES_H
extern double orient3d(
double *pa,
double *pb,
double *pc,
double *pd
);
extern double orient2d(
double *pa,
double *pb,
double *pc
);
extern void exactinit();

83
lib/include/openGJK/openGJK.h
View File

@ -1,83 +0,0 @@
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* 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 is the header file for the openGJK.c file. It defines the *
* openGJK function and its structures. *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
#ifndef __OPENGJK_H__
#define __OPENGJK_H__
#include <stdio.h>
#include <stdlib.h>
#include "math.h"
/**
* @brief Macro that implements the CompareSign function (see paper).
*/
#define SAMESIGN( a, b ) ( (a>0) == (b>0) )
/**
* @brief Structure of a body.
*/
struct bd {
int numpoints; /**< Number of points defining the body. */
double **coord; /**< Pointer to pointer to the points' coordinates. */
double s [3]; /**< Support mapping computed last. */
};
/**
* @brief Structure for a simplex.
*/
struct simplex {
int nvrtx ; /**< Number of simplex's vertices. */
double vrtx [4][3]; /**< Coordinates of simplex's vertices. */
int wids [4]; /**< Label of the simplex's vertices. */
double lambdas [4]; /**< Barycentric coordiantes for each vertex. */
double p [4][3]; /**< Points of P that form the simplex */
double q [4][3]; /**< Points of Q that form the simplex */
};
/**
* @brief The GJK algorithm which returns the minimum distance between
* two bodies.
*/
extern double gjk( struct bd, struct bd, struct simplex * ) ;
#endif

1009
lib/src/openGJK.c
View File

File diff suppressed because it is too large Load Diff

945
src/openGJK.c Normal file
View File

@ -0,0 +1,945 @@
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* 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 *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
#include "openGJK/openGJK.h"
/* If instricuted, compile a mex function for Matlab. */
#ifdef MATLABDOESMEXSTUFF
#include "mex.h"
#else
#define mexPrintf printf
#endif
#define eps_rel22 1e-5
#define eps_tot22 1e-14
/* Select distance sub-algorithm */
#define norm2(a) (a[0]*a[0]+a[1]*a[1]+a[2]*a[2])
#define dotProduct(a, b) (a[0]*b[0]+a[1]*b[1]+a[2]*b[2])
#define S3Dregion1234() v[0] = 0;\
v[1] = 0;\
v[2] = 0;\
s->nvrtx = 4;
#define select_1ik() s->nvrtx = 3;\
for (t = 0; t < 3; t++)\
s->vrtx[2][t] = s->vrtx[3][t];\
for (t = 0; t < 3; t++)\
s->vrtx[1][t] = si[t];\
for (t = 0; t < 3; t++)\
s->vrtx[0][t] = sk[t];
#define select_1ij() s->nvrtx = 3;\
for (t = 0; t < 3; t++)\
s->vrtx[2][t] = s->vrtx[3][t];\
for (t = 0; t < 3; t++)\
s->vrtx[1][t] = si[t];\
for (t = 0; t < 3; t++)\
s->vrtx[0][t] = sj[t];
#define select_1jk() s->nvrtx = 3;\
for (t = 0; t < 3; t++)\
s->vrtx[2][t] = s->vrtx[3][t];\
for (t = 0; t < 3; t++)\
s->vrtx[1][t] = sj[t];\
for (t = 0; t < 3; t++)\
s->vrtx[0][t] = sk[t];
#define select_1i() s->nvrtx = 2;\
for (t = 0; t < 3; t++)\
s->vrtx[1][t] = s->vrtx[3][t];\
for (t = 0; t < 3; t++)\
s->vrtx[0][t] = si[t];
#define select_1j() s->nvrtx = 2;\
for (t = 0; t < 3; t++)\
s->vrtx[1][t] = s->vrtx[3][t];\
for (t = 0; t < 3; t++)\
s->vrtx[0][t] = sj[t];
#define select_1k() s->nvrtx = 2;\
for (t = 0; t < 3; t++)\
s->vrtx[1][t] = s->vrtx[3][t];\
for (t = 0; t < 3; t++)\
s->vrtx[0][t] = sk[t];
#define getvrtx(point, location) point[0] = s->vrtx[location][0];\
point[1] = s->vrtx[location][1];\
point[2] = s->vrtx[location][2];
#define calculateEdgeVector(p1p2, p2) p1p2[0] = p2[0] - s->vrtx[3][0];\
p1p2[1] = p2[1] - s->vrtx[3][1];\
p1p2[2] = p2[2] - s->vrtx[3][2];
#define S1Dregion1() v[0] = s->vrtx[1][0];\
v[1] = s->vrtx[1][1];\
v[2] = s->vrtx[1][2];\
s->nvrtx = 1;\
s->vrtx[0][0] = s->vrtx[1][0];\
s->vrtx[0][1] = s->vrtx[1][1];\
s->vrtx[0][2] = s->vrtx[1][2];
#define S2Dregion1() v[0] = s->vrtx[2][0];\
v[1] = s->vrtx[2][1];\
v[2] = s->vrtx[2][2];\
s->nvrtx = 1;\
s->vrtx[0][0] = s->vrtx[2][0];\
s->vrtx[0][1] = s->vrtx[2][1];\
s->vrtx[0][2] = s->vrtx[2][2];
#define S2Dregion12() s->nvrtx = 2;\
s->vrtx[0][0] = s->vrtx[2][0];\
s->vrtx[0][1] = s->vrtx[2][1];\
s->vrtx[0][2] = s->vrtx[2][2];
#define S2Dregion13() s->nvrtx = 2;\
s->vrtx[1][0] = s->vrtx[2][0];\
s->vrtx[1][1] = s->vrtx[2][1];\
s->vrtx[1][2] = s->vrtx[2][2];
#define S3Dregion1() v[0] = s1[0];\
v[1] = s1[1];\
v[2] = s1[2];\
s->nvrtx = 1;\
s->vrtx[0][0] = s1[0];\
s->vrtx[0][1] = s1[1];\
s->vrtx[0][2] = s1[2];
inline static double determinant(const double *p, const double *q, const double *r) {
return p[0] * ((q[1] * r[2]) - (r[1] * q[2])) - p[1] * (q[0] * r[2] - r[0] * q[2]) + p[2] * (q[0] * r[1] - r[0] * q[1]);
}
inline static void crossProduct(const double *a, const double *b, double *c)
{
c[0] = a[1] * b[2] - a[2] * b[1];
c[1] = a[2] * b[0] - a[0] * b[2];
c[2] = a[0] * b[1] - a[1] * b[0];
}
inline static void projectOnLine(const double *p, const double *q, double *v)
{
double pq[3];
double tmp;
pq[0] = p[0] - q[0];
pq[1] = p[1] - q[1];
pq[2] = p[2] - q[2];
tmp = dotProduct(p, pq) / dotProduct(pq, pq);
for (int i = 0; i < 3; i++)
v[i] = p[i] - pq[i] * tmp;
}
inline static void projectOnPlane(const double *p, const double *q, const double *r, double *v)
{
double n[3], pq[3], pr[3];
double tmp;
for (int i = 0; i < 3; i++)
pq[i] = p[i] - q[i];
for (int i = 0; i < 3; i++)
pr[i] = p[i] - r[i];
crossProduct(pq, pr, n);
tmp = dotProduct(n, p) / dotProduct(n, n);
for (int i = 0; i < 3; i++)
v[i] = n[i] * tmp;
}
inline static int hff1(const double *p, const double *q)
{
double tmp = 0;
#pragma omp simd reduction(+:tmp)
for (int i = 0; i < 3; i++)
tmp += (p[i] * p[i] - p[i] * q[i]);
if (tmp > 0)
return 1; // keep q
return 0;
}
inline static int hff2(const double *p, const double *q, const double *r)
{
double ntmp[3];
double n[3], pq[3], pr[3];
double tmp = 0;
for (int i = 0; i < 3; i++)
pq[i] = q[i] - p[i];
for (int i = 0; i < 3; i++)
pr[i] = r[i] - p[i];
crossProduct(pq, pr, ntmp);
crossProduct(pq, ntmp, n);
#pragma omp simd reduction(+:tmp)
for (int i = 0; i < 3; i++)
tmp = tmp + (p[i] * n[i]);
if (tmp < 0)
return 1; // Discard r
return 0;
}
inline static int hff3(const double *p, const double *q, const double *r)
{
double n[3], pq[3], pr[3];
double tmp = 0;
for (int i = 0; i < 3; i++)
pq[i] = q[i] - p[i];
for (int i = 0; i < 3; i++)
pr[i] = r[i] - p[i];
crossProduct(pq, pr, n);
#pragma omp simd reduction(+:tmp)
for (int i = 0; i < 3; i++)
tmp = tmp + (p[i] * n[i]);
if (tmp > 0)
return 0; // discard s
return 1;
}
inline static void S1D(struct simplex * s, double *v)
{
double *s1p = s->vrtx[1];
double *s2p = s->vrtx[0];
if (hff1(s1p, s2p)) {
projectOnLine(s1p, s2p, v); // Update v, no need to update s
return; // Return V{1,2}
}
else {
S1Dregion1(); // Update v and s
return; // Return V{1}
}
}
inline static void S2D(struct simplex * s, double *v)
{
double *s1p = s->vrtx[2];
double *s2p = s->vrtx[1];
double *s3p = s->vrtx[0];
int hff1f_s12 = hff1(s1p, s2p);
int hff1f_s13 = hff1(s1p, s3p);
int hff2f_23 = !hff2(s1p, s2p, s3p);
int hff2f_32 = !hff2(s1p, s3p, s2p);
if (hff1f_s12) {
if (hff2f_23) {
if (hff1f_s13) {
if (hff2f_32) {
projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c
return; // Return V{1,2,3}
}
else
{
projectOnLine(s1p, s3p, v); // Update v
S2Dregion13(); // Update s
return; // Return V{1,3}
}
}
else
{
projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c
return; // Return V{1,2,3}
}
}
else
{
projectOnLine(s1p, s2p, v); // Update v
S2Dregion12(); // Update s
return; // Return V{1,2}
}
}
else if (hff1f_s13) {
if (hff2f_32) {
projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c
return; // Return V{1,2,3}
}
else
{
projectOnLine(s1p, s3p, v); // Update v
S2Dregion13(); // Update s
return; // Return V{1,3}
}
}
else {
S2Dregion1(); // Update s and v
return; // Return V{1}
}
}
inline static void S3D(struct simplex * s, double *v) {
double s1[3], s2[3], s3[3], s4[3], s1s2[3], s1s3[3], s1s4[3];
double si[3], sj[3], sk[3];
int testLineThree, testLineFour, testPlaneTwo, testPlaneThree, testPlaneFour, dotTotal;
int i, j, k, t;
getvrtx(s1, 3);
getvrtx(s2, 2);
getvrtx(s3, 1);
getvrtx(s4, 0);
calculateEdgeVector(s1s2, s2);
calculateEdgeVector(s1s3, s3);
calculateEdgeVector(s1s4, s4);
int hff1_tests[3];
hff1_tests[2] = hff1(s1, s2);
hff1_tests[1] = hff1(s1, s3);
hff1_tests[0] = hff1(s1, s4);
testLineThree = hff1(s1, s3);
testLineFour = hff1(s1, s4);
dotTotal = hff1(s1, s2) + testLineThree + testLineFour;
if (dotTotal == 0) { /* case 0.0 -------------------------------------- */
S3Dregion1();
return;
}
double det134 = determinant(s1s3, s1s4, s1s2);
int sss;
if (det134 > 0) {
sss = 0;
}
else {
sss = 1;
}
testPlaneTwo = hff3(s1, s3, s4) - sss;
testPlaneTwo = testPlaneTwo * testPlaneTwo;
testPlaneThree = hff3(s1, s4, s2) - sss;
testPlaneThree = testPlaneThree * testPlaneThree;
testPlaneFour = hff3(s1, s2, s3) - sss;
testPlaneFour = testPlaneFour * testPlaneFour;
switch (testPlaneTwo + testPlaneThree + testPlaneFour) {
case 3:
S3Dregion1234();
break;
case 2:
// Only one facing the oring
// 1,i,j, are the indices of the points on the triangle and remove k from simplex
s->nvrtx = 3;
if (!testPlaneTwo) { // k = 2; removes s2
for (i = 0; i < 3; i++)
s->vrtx[2][i] = s->vrtx[3][i];
}
else if (!testPlaneThree) {// k = 1; // removes s3
for (i = 0; i < 3; i++)
s->vrtx[1][i] = s2[i];
for (i = 0; i < 3; i++)
s->vrtx[2][i] = s->vrtx[3][i];
}
else if (!testPlaneFour) { // k = 0; // removes s4 and no need to reorder
for (i = 0; i < 3; i++)
s->vrtx[0][i] = s3[i];
for (i = 0; i < 3; i++)
s->vrtx[1][i] = s2[i];
for (i = 0; i < 3; i++)
s->vrtx[2][i] = s->vrtx[3][i];
}
// Call S2D
S2D(s, v);
break;
case 1:
// Two triangles face the origins:
// The only positive hff3 is for triangle 1,i,j, therefore k must be in the solution as it supports the the point of minimum norm.
// 1,i,j, are the indices of the points on the triangle and remove k from simplex
s->nvrtx = 3;
if (testPlaneTwo) {
k = 2; // s2
i = 1;
j = 0;
}
else if (testPlaneThree) {
k = 1; // s3
i = 0;
j = 2;
}
else {
k = 0; // s4
i = 2;
j = 1;
}
getvrtx(si, i);
getvrtx(sj, j);
getvrtx(sk, k);
if (dotTotal == 1) {
if (hff1_tests[k]) {
if (!hff2(s1, sk, si)) {
select_1ik();
projectOnPlane(s1, si, sk, v);
}
else if (!hff2(s1, sk, sj)) {
select_1jk();
projectOnPlane(s1, sj, sk, v);
}
else {
select_1k(); // select region 1i
projectOnLine(s1, sk, v);
}
}
else if (hff1_tests[i]) {
if (!hff2(s1, si, sk)) {
select_1ik();
projectOnPlane(s1, si, sk, v);
}
else {
select_1i(); // select region 1i
projectOnLine(s1, si, v);
}
}
else {
if (!hff2(s1, sj, sk)) {
select_1jk();
projectOnPlane(s1, sj, sk, v);
}
else {
select_1j(); // select region 1i
projectOnLine(s1, sj, v);
}
}
}
else if (dotTotal == 2) {
// Two edges have positive hff1, meaning that for two edges the origin's project fall on the segement.
// Certainly the edge 1,k supports the the point of minimum norm, and so hff1_1k is positive
if (hff1_tests[i]) {
if (!hff2(s1, sk, si))
if (!hff2(s1, si, sk)) {
select_1ik(); // select region 1ik
projectOnPlane(s1, si, sk, v);
}
else {
select_1k(); // select region 1k
projectOnLine(s1, sk, v);
}
else {
if (!hff2(s1, sk, sj)) {
select_1jk(); // select region 1jk
projectOnPlane(s1, sj, sk, v);
}
else {
select_1k(); // select region 1k
projectOnLine(s1, sk, v);
}
}
}
else if (hff1_tests[j]) {// there is no other choice
if (!hff2(s1, sk, sj))
if (!hff2(s1, sj, sk)) {
select_1jk(); // select region 1jk
projectOnPlane(s1, sj, sk, v);
}
else {
select_1j(); // select region 1j
projectOnLine(s1, sj, v);
}
else {
if (!hff2(s1, sk, si)) {
select_1ik(); // select region 1ik
projectOnPlane(s1, si, sk, v);
}
else {
select_1k(); // select region 1k
projectOnLine(s1, sk, v);
}
}
}
else {
// ERROR;
}
}
else if (dotTotal == 3) {
// MM : ALL THIS HYPHOTESIS IS FALSE
// sk is s.t. hff3 for sk < 0. So, sk must support the origin because there are 2 triangles facing the origin.
int hff2_ik = hff2(s1,si,sk);
int hff2_jk = hff2(s1,sj,sk);
int hff2_ki = hff2(s1,sk,si);
int hff2_kj = hff2(s1,sk,sj);
if (hff2_ki == 0 && hff2_kj == 0){
mexPrintf("\n\n UNEXPECTED VALUES!!! \n\n");
}
if (hff2_ki == 1 && hff2_kj == 1){
select_1k();
projectOnLine(s1, sk, v);
}
else if (hff2_ki) {
// discard i
if (hff2_jk){
// discard k
select_1j();
projectOnLine(s1, sj, v);
}
else{
select_1jk();
projectOnPlane(s1, sk, sj, v);
}
}
else {
// discard j
if (hff2_ik){
// discard k
select_1i();
projectOnLine(s1, si, v);
}
else{
select_1ik();
projectOnPlane(s1, sk, si, v);
}
}
}
break;
case 0:
// The origin is outside all 3 triangles
if (dotTotal == 1) {
// Here si is set such that hff(s1,si) > 0
if (testLineThree) {
k = 2;
i = 1; // s3
j = 0;
}
else if (testLineFour) {
k = 1; // s3
i = 0;
j = 2;
}
else {
k = 0;
i = 2; // s2
j = 1;
}
getvrtx(si, i);
getvrtx(sj, j);
getvrtx(sk, k);
if (!hff2(s1, si, sj)) {
select_1ij();
projectOnPlane(s1, si, sj, v);
}
else if (!hff2(s1, si, sk)) {
select_1ik();
projectOnPlane(s1, si, sk, v);
}
else {
select_1i();
projectOnLine(s1, si, v);
}
}
else if (dotTotal == 2) {
// Here si is set such that hff(s1,si) < 0
s->nvrtx = 3;
if (!testLineThree) {
k = 2;
i = 1; // s3
j = 0;
}
else if (!testLineFour) {
k = 1;
i = 0; // s4
j = 2;
}
else {
k = 0;
i = 2; // s2
j = 1;
}
getvrtx(si, i);
getvrtx(sj, j);
getvrtx(sk, k);
if (!hff2(s1, sj, sk)) {
if (!hff2(s1, sk, sj)) {
select_1jk(); // select region 1jk
projectOnPlane(s1, sj, sk, v);
}
else if (!hff2(s1, sk, si)) {
select_1ik();
projectOnPlane(s1, sk, si, v);
}
else {
select_1k();
projectOnLine(s1, sk, v);
}
}
else if (!hff2(s1, sj, si)) {
select_1ij();
projectOnPlane(s1, si, sj, v);
}
else {
select_1j();
projectOnLine(s1, sj, v);
}
}
break;
default:
mexPrintf("\nERROR:\tunhandled");
}
}
inline static void support(struct bd *body, const double *v) {
double s, maxs;
double *vrt;
int better = -1;
maxs = dotProduct(body->s, v);
for (int i = 0; i < body->numpoints; ++i) {
vrt = body->coord[i];
s = dotProduct(vrt, v);
if (s > maxs) {
maxs = s;
better = i;
}
}
if (better != -1) {
body->s[0] = body->coord[better][0];
body->s[1] = body->coord[better][1];
body->s[2] = body->coord[better][2];
}
}
inline static void subalgorithm(struct simplex *s, double *v) {
switch (s->nvrtx) {
case 4:
S3D(s, v);
break;
case 3:
S2D(s, v);
break;
case 2:
S1D(s, v);
break;
default:
mexPrintf("\nERROR:\t invalid simplex\n");
}
}
double gjk(struct bd bd1, struct bd bd2, struct simplex *s) {
int k = 0; /**< Iteration counter */
int i; /**< General purpose counter */
int mk = 5000; /**< Maximum number of iterations of the GJK algorithm */
int absTestin;
double norm2Wmax = 0;
double tesnorm;
double v[3]; /**< Search direction */
double vminus[3]; /**< Search direction * -1 */
double w[3]; /**< Vertex on CSO boundary given by the difference of support functions on both bodies */
double eps_rel = eps_rel22; /**< Tolerance on relative */
double eps_rel2 = eps_rel * eps_rel;
double eps_tot = eps_tot22;
int exeedtol_rel = 0; /**< Flag for 1st exit condition */
int nullV = 0;
#ifdef DEBUG
mexPrintf("Num points A = %i \n", bd1.numpoints);
mexPrintf("Num points B = %i \n", bd2.numpoints);
for (i = 0; i < bd1.numpoints; ++i) {
for (int j = 0; j < 3; j++) {
mexPrintf("%.4f ", bd1.coord[i][j]);
}
mexPrintf("\n");
}
for (i = 0; i < bd2.numpoints; ++i) {
for (int j = 0; j < 3; j++) {
mexPrintf("%.4f ", bd2.coord[i][j]);
}
mexPrintf("\n");
}
#endif
/* Initialise search direction */
v[0] = bd1.coord[0][0] - bd2.coord[0][0];
v[1] = bd1.coord[0][1] - bd2.coord[0][1];
v[2] = bd1.coord[0][2] - bd2.coord[0][2];
/* Inialise simplex */
s->nvrtx = 1;
for (int t = 0; t < 3; ++t)
s->vrtx[0][t] = v[t];
for (int t = 0; t < 3; ++t)
bd1.s[t] = bd1.coord[0][t];
for (int t = 0; t < 3; ++t)
bd2.s[t] = bd2.coord[0][t];
/* Begin GJK iteration */
do {
k++;
/* Update negative search direction */
for (int t = 0; t < 3; ++t)
vminus[t] = -v[t];
/* Support function */
support(&bd1, vminus);
support(&bd2, v);
for (int t = 0; t < 3; ++t)
w[t] = bd1.s[t] - bd2.s[t];
/* Test first exit condition (new point already in simplex/can't move further) */
exeedtol_rel = (norm2(v) - dotProduct(v, w)) <= eps_rel2 * norm2(v);
if (exeedtol_rel) {
break;
}
nullV = norm2(v) < eps_rel2;
if (nullV) {
break;
}
/* Add new vertex to simplex */
i = s->nvrtx;
for (int t = 0; t < 3; ++t)
s->vrtx[i][t] = w[t];
s->nvrtx++;
/* Invoke distance sub-algorithm */
subalgorithm(s, v);
/* Test */
for (int jj = 0; jj < s->nvrtx; jj++) {
tesnorm = norm2(s->vrtx[jj]);
if (tesnorm > norm2Wmax) {
norm2Wmax = tesnorm;
}
}
absTestin = (norm2(v) <= (eps_tot * eps_tot * norm2Wmax));
if (absTestin)
break;
} while ((s->nvrtx != 4) && (k != mk));
if (k == mk) {
mexPrintf("\n * * * * * * * * * * * * MAXIMUM ITERATION NUMBER REACHED!!! * * * * * * * * * * * * * * \n");
}
return sqrt(norm2(v));
}
#ifdef MATLABDOESMEXSTUFF
/**
* @brief Mex function for Matlab.
*/
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
double *inCoordsA;
double *inCoordsB;
size_t nCoordsA;
size_t nCoordsB;
int i;
double *distance;
int c = 3;
int count = 0;
double**arr1;
double**arr2;
/**************** PARSE INPUTS AND OUTPUTS **********************/
/*----------------------------------------------------------------*/
/* Examine input (right-hand-side) arguments. */
if (nrhs != 2) {
mexErrMsgIdAndTxt("MyToolbox:gjk:nrhs", "Two inputs required.");
}
/* Examine output (left-hand-side) arguments. */
if (nlhs != 1) {
mexErrMsgIdAndTxt("MyToolbox:gjk:nlhs", "One output required.");
}
/* make sure the two input arguments are any numerical type */
/* .. first input */
if (!mxIsNumeric(prhs[0])) {
mexErrMsgIdAndTxt("MyToolbox:gjk:notNumeric", "Input matrix must be type numeric.");
}
/* .. second input */
if (!mxIsNumeric(prhs[1])) {
mexErrMsgIdAndTxt("MyToolbox:gjk:notNumeric", "Input matrix must be type numeric.");
}
/* make sure the two input arguments have 3 columns */
/* .. first input */
if (mxGetM(prhs[0]) != 3) {
mexErrMsgIdAndTxt("MyToolbox:gjk:notColumnVector", "First input must have 3 columns.");
}
/* .. second input */
if (mxGetM(prhs[1]) != 3) {
mexErrMsgIdAndTxt("MyToolbox:gjk:notColumnVector", "Second input must have 3 columns.");
}
/*----------------------------------------------------------------*/
/* CREATE DATA COMPATIBLE WITH MATALB */
/* create a pointer to the real data in the input matrix */
inCoordsA = mxGetPr(prhs[0]);
inCoordsB = mxGetPr(prhs[1]);
/* get the length of each input vector */
nCoordsA = mxGetN(prhs[0]);
nCoordsB = mxGetN(prhs[1]);
/* Create output */
plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
/* get a pointer to the real data in the output matrix */
distance = mxGetPr(plhs[0]);
/* Copy data from Matlab's vectors into two new arrays */
arr1 = (double **)mxMalloc(sizeof(double *) * (int)nCoordsA);
arr2 = (double **)mxMalloc(sizeof(double *) * (int)nCoordsB);
for (i = 0; i < nCoordsA; i++)
arr1[i] = &inCoordsA[i * 3];
for (i = 0; i < nCoordsB; i++)
arr2[i] = &inCoordsB[i * 3];
/*----------------------------------------------------------------*/
/* POPULATE BODIES' STRUCTURES */
struct bd bd1; /* Structure of body A */
struct bd bd2; /* Structure of body B */
/* Assign number of vertices to each body */
bd1.numpoints = (int)nCoordsA;
bd2.numpoints = (int)nCoordsB;
bd1.coord = arr1;
bd2.coord = arr2;
/*----------------------------------------------------------------*/
/*CALL COMPUTATIONAL ROUTINE */
struct simplex s;
s.nvrtx = 0;
/* Compute squared distance using GJK algorithm */
distance[0] = gjk(bd1, bd2, &s);
mxFree(arr1);
mxFree(arr2);
}
#endif
/**
* @brief Invoke this function from C# applications
*/
double csFunction(int nCoordsA, double *inCoordsA, int nCoordsB, double *inCoordsB)
{
double distance = 0;
int i, j;
/*----------------------------------------------------------------*/
/* POPULATE BODIES' STRUCTURES */
struct bd bd1; /* Structure of body A */
struct bd bd2; /* Structure of body B */
/* Assign number of vertices to each body */
bd1.numpoints = (int)nCoordsA;
bd2.numpoints = (int)nCoordsB;
double **pinCoordsA = (double **)malloc(bd1.numpoints * sizeof(double *));
for (i = 0; i < bd1.numpoints; i++)
pinCoordsA[i] = (double *)malloc(3 * sizeof(double));
for (i = 0; i < 3; i++)
for (j = 0; j < bd1.numpoints; j++)
pinCoordsA[j][i] = inCoordsA[i*bd1.numpoints + j];
double **pinCoordsB = (double **)malloc(bd2.numpoints * sizeof(double *));
for (i = 0; i < bd2.numpoints; i++)
pinCoordsB[i] = (double *)malloc(3 * sizeof(double));
for (i = 0; i < 3; i++)
for (j = 0; j < bd2.numpoints; j++)
pinCoordsB[j][i] = inCoordsB[i*bd2.numpoints + j];
bd1.coord = pinCoordsA;
bd2.coord = pinCoordsB;
/*----------------------------------------------------------------*/
/*CALL COMPUTATIONAL ROUTINE */
struct simplex s;
/* Initialise simplex as empty */
s.nvrtx = 0;
/* Compute squared distance using GJK algorithm */
distance = gjk(bd1, bd2, &s);
for (i = 0; i < bd1.numpoints; i++)
free(pinCoordsA[i]);
free(pinCoordsA);
for (i = 0; i < bd2.numpoints; i++)
free(pinCoordsB[i]);
free(pinCoordsB);
return distance;
}