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

178
CMakeLists.txt
View File

@ -1,94 +1,84 @@
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
# ##### # # # # # ##### # # # #
# #### ##### ###### # # # # # # # # # #### ##### ###### # # # # # # # #
# # # # # # ## # # # # # # # # # # # # ## # # # # # #
# # # # # ##### # # # # #### # ### # # # # # # ##### # # # # #### # ### #
# # # ##### # # # # # # # # # # # # # # ##### # # # # # # # # # # #
# # # # # # ## # # # # # # # # # # # # # ## # # # # # # #
# #### # ###### # # ##### ##### # # # # #### # ###### # # ##### ##### # # #
# # # #
# This file is part of openGJK. # # This file is part of openGJK. #
# # # #
# openGJK is free software: you can redistribute it and/or modify # # openGJK is free software: you can redistribute it and/or modify #
# it under the terms of the GNU General Public License as published by # # it under the terms of the GNU General Public License as published by #
# the Free Software Foundation, either version 3 of the License, or # # the Free Software Foundation, either version 3 of the License, or #
# any later version. # # any later version. #
# # # #
# openGJK is distributed in the hope that it will be useful, # # openGJK is distributed in the hope that it will be useful, #
# but WITHOUT ANY WARRANTY; without even the implied warranty of # # but WITHOUT ANY WARRANTY; without even the implied warranty of #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The # # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The #
# GNU General Public License for more details. # # GNU General Public License for more details. #
# # # #
# You should have received a copy of the GNU General Public License # # 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 # # openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
# Copyright (C) Mattia Montanari 2018 - 2019 # # Copyright (C) Mattia Montanari 2018 - 2019 #
# http://iel.eng.ox.ac.uk/?page_id=504 # # http://iel.eng.ox.ac.uk/?page_id=504 #
# # # #
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
cmake_minimum_required(VERSION 3.5) message( "[${PROJECT_NAME}] CMake setting ..")
project(openGJK)
message(STATUS "Version : " ${openGJK_VERSION} )
set(CMAKE_C_STANDARD 11) 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 )
# APPLY DEFAULT SETTINGS set( SOURCE_HEADS include/openGJK/openGJK.h)
if(NOT CMAKE_BUILD_TYPE)
message("[${CMAKE_PROJECT_NAME}] Use default CMAKE_BUILD_TYPE") IF(USE_PREDICATES)
set(CMAKE_BUILD_TYPE Release) # for adpative floating-point artim.
endif() set( SOURCE_FILES ${SOURCE_FILES} ext/predicates.c)
set( SOURCE_HEADS ${SOURCE_HEADS} ext/predicates.h)
# Add flag for adpative floating-point artim.
# PLATFORM-SPECIFIC SETTING add_definitions(-DADAPTIVEFP)
if (UNIX) ENDIF()
find_library(M_LIB m)
set(CMAKE_C_FLAGS "-lm") IF(BUILD_STATIC_LIB)
set(CMAKE_CXX_FLAGS "-lm") message(STATUS "Library type: " Static )
else () add_library(${PROJECT_NAME} STATIC ${SOURCE_FILES} ${SOURCE_HEADS})
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=FALSE)
endif () ELSE()
message(STATUS "Library type: " Shared )
# COMPILER SETTING add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${SOURCE_HEADS})
IF(CMAKE_BUILD_TYPE MATCHES Release) add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=FALSE)
set(CMAKE_BUILD_TYPE Release) ENDIF(BUILD_STATIC_LIB)
ELSEIF(CMAKE_BUILD_TYPE MATCHES Debug)
set(CMAKE_BUILD_TYPE Debug) # PLATFORM-SPECIFIC SETTING
ENDIF() if (UNIX)
find_library(M_LIB m)
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") set(CMAKE_C_FLAGS "-lm")
# using GCC else ()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra -finline-functions") set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
set(CMAKE_CXX_FLAGS_DEBUG "-g -DDEBUG") endif ()
set(CMAKE_CXX_FLAGS_RELEASE "-O3")
find_package(OpenMP)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -finline-functions") if (OPENMP_FOUND)
set(CMAKE_C_FLAGS_DEBUG "-g -DDEBUG") set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_C_FLAGS_RELEASE "-O3") set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
endif()
add_compile_options(-static-libgcc -static-libstdc++ )
add_definitions(-DMT) # ADD DEFAULT COMPILER FLAGS
elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") include(CompilerFlags)
# using Visual Studio C++
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4131 /wd4701 /wd4255 /wd4710 /wd4820 /wd4711 /wd5045") # Link include file
set(CMAKE_CXX_FLAGS_DEBUG "-DDEBUG /D_DEBUG /MDd /Zi /Ob0 /Od /RTC1") target_include_directories( ${PROJECT_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
set(CMAKE_CXX_FLAGS_RELEASE "/Ox") target_link_libraries(${PROJECT_NAME} ${CMOCKA_LIBRARY} OpenMP::OpenMP_C )
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /wd4131 /wd4701 /wd4255 /wd4710 /wd4820 /wd4711 /wd5045") IF(USE_PREDICATES)
set(CMAKE_C_FLAGS_DEBUG "-DDEBUG /D_DEBUG /MDd /Zi /Ob0 /Od /RTC1") # for adpative floating-point artim.
set(CMAKE_C_FLAGS_RELEASE "/Ox") target_include_directories( ${PROJECT_NAME}
PUBLIC ${PROJECT_SOURCE_DIR}/ext
set(CMAKE_SUPPRESS_REGENERATION true) )
ENDIF()
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)

1348
COPYING
View File

File diff suppressed because it is too large Load Diff

300
INSTALL.md
View File

@ -1,151 +1,151 @@
# How to compile openGJK # How to compile openGJK
Using openGJK is very simple. This guide will help you getting started compiling and using openGJK. Using openGJK is very simple. This guide will help you getting started compiling and using openGJK.
## Requirements ## Requirements
### Common requirements ### Common requirements
1. A C compiler 1. A C compiler
2. [CMake](http://www.cmake.org) version 3.5 or above 2. [CMake](http://www.cmake.org) version 3.5 or above
## Building ## Building
First, you need to configure the compilation, using CMake. First, you need to configure the compilation, using CMake.
1. Go inside the `build` dir. Create it if it doesn't exist. 1. Go inside the `build` dir. Create it if it doesn't exist.
2. Move into `build` dir and use `cmake ..`. On Windows you can specify `cmake -G "Visual Studio 15 2017 Win64" ..`, on Unix `cmake -G "Unix Makefiles" ..`. 2. Move into `build` dir and use `cmake ..`. On Windows you can specify `cmake -G "Visual Studio 15 2017 Win64" ..`, on Unix `cmake -G "Unix Makefiles" ..`.
### CMake standard options ### CMake standard options
- CMAKE_BUILD_TYPE: The type of build (can be Debug or Release) - CMAKE_BUILD_TYPE: The type of build (can be Debug or Release)
- CMAKE_C_COMPILER: The path to the C compiler - CMAKE_C_COMPILER: The path to the C compiler
### CMake options defined for openGJK ### CMake options defined for openGJK
Options are defined in the following files: Options are defined in the following files:
- CmakeOptions.cmake - CmakeOptions.cmake
They can be changed with the -D option: They can be changed with the -D option:
`cmake -DVERSION_ACCURATE=ON ..` `cmake -DVERSION_ACCURATE=ON ..`
In addition to passing options on the command line, you can browse and edit In addition to passing options on the command line, you can browse and edit
CMake options using `cmakesetup` (Windows), `cmake-gui` or `ccmake` (GNU/Linux CMake options using `cmakesetup` (Windows), `cmake-gui` or `ccmake` (GNU/Linux
and MacOS X). and MacOS X).
- Go to the build dir - Go to the build dir
- On Windows: run `cmakesetup` - On Windows: run `cmakesetup`
- On GNU/Linux and MacOS X: run `ccmake ..` - On GNU/Linux and MacOS X: run `ccmake ..`
### Install and run ### Install and run
If all above building commands were executed from `build`, the openGJK library can be found in the `build/src` directory. If all above building commands were executed from `build`, the openGJK library can be found in the `build/src` directory.
You can run the binaries in `build/examples/*`. You can run the binaries in `build/examples/*`.
To install the library 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). To install the library 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).
## Testing ## Testing
TO REWRITE!! TO REWRITE!!
As mention above you can turn on the unit tests and make it possible to easily As mention above you can turn on the unit tests and make it possible to easily
execute them: execute them:
`cmake -DCMAKE_BUILD_TYPE=Debug -DUNIT_TESTING=ON ..` `cmake -DCMAKE_BUILD_TYPE=Debug -DUNIT_TESTING=ON ..`
After that you can simply call `make test` in the build directory or if you After that you can simply call `make test` in the build directory or if you
want more output simply call `ctest -V`. want more output simply call `ctest -V`.
If you want to enable the generation of coverage files you can do this by If you want to enable the generation of coverage files you can do this by
using the following options: using the following options:
`cmake -DCMAKE_BUILD_TYPE=Profiling -DUNIT_TESTING=ON ..` `cmake -DCMAKE_BUILD_TYPE=Profiling -DUNIT_TESTING=ON ..`
After building it you will see that you have several coverage options in After building it you will see that you have several coverage options in
`make help` `make help`
You should have `make ExperimentalCoverage` and running it will create You should have `make ExperimentalCoverage` and running it will create
coverage files. The result is stored in Testing directory. coverage files. The result is stored in Testing directory.
## Examples ## Examples
This section presents three examples on how to use openGJK with C, C# and Matlab. This section presents three examples on how to use openGJK with C, C# and Matlab.
### C ### C
This example illustrates how to include openGJK in an existing C This example illustrates how to include openGJK in an existing C
program. program.
All files for the example are in the `example1_c` folder. The executable built with 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, CMake reads the coordinates of two polytopes from the input files,
respectively userP.dat and userQ.dat, and computes the minimum distance respectively userP.dat and userQ.dat, and computes the minimum distance
between them. between them.
Notice that the input files must be in the folder from which the executable Notice that the input files must be in the folder from which the executable
is launched, otherwise an error is returned. is launched, otherwise an error is returned.
You can edit the coordinates in the input file to test different You can edit the coordinates in the input file to test different
polytopes; just remember to edit also the first number in the files polytopes; just remember to edit also the first number in the files
that corresponds to the numbers of vertices that the program will read. that corresponds to the numbers of vertices that the program will read.
### Matlab ### Matlab
This example illustrates how to invoke openGJK as a regular built-in This example illustrates how to invoke openGJK as a regular built-in
Matlab function. 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)). Matlab function. 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)).
Open Matlab and cd into the `example2_mex` folder. By running the Open Matlab and cd into the `example2_mex` folder. By running the
script `runme.m`, Matlab will first compile a mex file (telling you 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 about the name of the mex file generated) and will call the script
`main.m`. This invokes openGJK within Matlab and illustrates the `main.m`. This invokes openGJK within Matlab and illustrates the
result. result.
The mex file may be copied and called from any other Matlab project. The mex file may be copied and called from any other Matlab project.
### C# # ### C# #
This example illustrates how to invoke openGJK in an applications written in C#. You will need [mono](http://www.mono-project.com/) and Microsoft Visual Studio toolchain for C# on Windows. This example illustrates how to invoke openGJK in an applications written in C#. You will need [mono](http://www.mono-project.com/) and Microsoft Visual Studio toolchain for C# on Windows.
The only file required is in the `example3_csharp` folder. This can be compiled in Unix 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 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 is compiled for a specific architecture (usually x64), and this breaks the portability
of the .NET application compiled in this example. of the .NET application compiled in this example.
Below are the steps for compiling the C# application on Windows and Linux. Both 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. procedures assume the dynamic library of openGJK has been already compiled.
#### Compile on Windows #### Compile on Windows
1. Move into the folder `example3_csharp` and create a new folder `example3`. 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. 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)**. 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 4. Add to this project the `main.cs` file
5. Set x64 as the target platform, compile the application and run it. 5. Set x64 as the target platform, compile the application and run it.
#### Compile on Linux #### Compile on Linux
1. Move into the folder `example3_csharp` and create a new folder `example3`. 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. 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. 3. Move into that new folder and open a terminal.
4. Type `mcs -out:example3demo -d:UNIX ../main.cs` 4. Type `mcs -out:example3demo -d:UNIX ../main.cs`
5. Run the example by typing `mono example3demo` 5. Run the example by typing `mono example3demo`
## API user reference ## API user reference
```double gjk( struct bodyA, struct bodyB, struct simplex)``` ```double gjk( struct bodyA, struct bodyB, struct simplex)```
### Documentation ### Documentation
The folder `doc` contains a Doxygen file for generating the documentation of the whole 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. 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.
### Parameters ### Parameters
* **bodyA** The first body. * **bodyA** The first body.
* **bodyB** The second body. * **bodyB** The second body.
* **simplex** The simplex used the GJK algorithm at the first iteration. * **simplex** The simplex used the GJK algorithm at the first iteration.
### Returns ### Returns
* **double** the minimum distance between bodyA and bodyB. * **double** the minimum distance between bodyA and bodyB.
### Description ### Description
The function `gjk` computes the minimum Euclidean distance between two bodies using the 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. GJK algorithm. Note that the simplex used at the first iteration may be initialised by the user, but this is not necessary.

1348
LICENSE
View File

File diff suppressed because it is too large Load Diff

325
README.md
View File

@ -1,230 +1,95 @@
openGJK {#mainpage} openGJK {#mainpage}
======= =======
The openGJK library uses the Gilbert-Johnson-Keerthi (GJK) algorithm to OpenGJK implements a new version of the Gilbert-Johnson-Keerthi (GJK) algorithm to
compute the minimum distance between convex polytopes. The 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.
implementation follows the description presented in
"[Improving the GJK Algorithm for Faster and More Reliable Distance 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)" and has been tested Queries Between Convex Objects. ACM Trans. on Graph. 2017](https://dl.acm.org/citation.cfm?id=3083724)".
on Unix and Windows systems for C, C# and Matlab programs.
This library offers researchers a tool that works When should I use openGJK?
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. OpenGJK is designed with speed, accuracy and robustness in mind and is therefore suitable for engineering, robotics and computer graphics simulations.
This library is not optimised for production, but it does provide a comprehensive and robust implementation. It is sufficiently fast for Basically, openGJK can be used in any application where the distance between **any convex polytope** is required.
most applications, and you can also build from here to suite your own
application. For instance, openGJK is not for incremental and is not Compile and run
for NURBS, but it offers a good starting point for such specific ---------------
applications.
To compile the OpenGJK library create a build dir,
## Getting Started and in the build dir call 'cmake ..' followed by 'make'. More details can be found in the INSTALL file.
Using openGJK is very simple. This guide will help you getting
started compiling and using openGJK. There are examples for C, C# and Matlab in the `examples` folder. The INSTALL file provides information on how to run the examples.
### When should I use openGJK? Repository content
------------------
OpenGJK is designed with accuracy and robustness in mind and is
suitable for engineering simulations. Good use of this library This repository contains the following files and folders:
include the finite element method (FEM) and discrete element method (DEM).
```
Basically, openGJK can measure the distance between **any convex polytope**. For example: │ CMakeLists.txt
- line segments │ README.md
- triangles │
- tetrahedrons ├───doc
- cubes. │ openGJKcustomfooter.html
│ openGJKcustomheader.html
### Installing the openGJK library │ openGJKcustomstyle.css
│ Doxyfile
#### Prerequisites │ oxfordLogo.jpg
│
1. A compiler (gnu or Microsoft Visual Studio for C) ├───example1_c
2. CMake version 3.5 or above │ CMakeLists.txt
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)). │ main.c
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. │ userP.dat
│ userQ.dat
### Installation │
├───example2_mex
There are CMake files for compiling openGJK in the usual │ main.m
way: │ runme.m
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, ├───example3_csharp
on Windows you can type `cmake -G "Visual Studio 15 2017 Win64" ..`, on Unix `cmake -G "Unix Makefiles" ..`. │ main.cs
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 └───lib
for the C example. For Matlab and C# applications, see sections below. │ CMakeLists.txt
│
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). ├───ext
│ predicates.c
### Automated documentation │ predicates.h
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. ├───include
│ └───openGJK
## API user reference │ openGJK.h
│
```double gjk( struct bodyA, struct bodyB, struct simplex)``` └───src
openGJK.c
### Parameters ```
* **bodyA** The first body.
* **bodyB** The second body. More information
* **simplex** The simplex used the GJK algorithm at the first iteration. ----------------
### Returns [OpenGJK](http://iel.eng.ox.ac.uk/?page_id=504) was developed at the Impact Engineering Laboratory, University of Oxford.
* **double** the minimum distance between bodyA and bodyB.
### Description A clear presentation of the GJK algorithm can be found in the
The function `gjk` computes the minimum Euclidean distance between two bodies using the book by **Van der Bergen** *Collision Detection in Interactive 3D
GJK algorithm. Note that the simplex used at the first iteration may be initialised by the user, but this is not necessary. Environments*, edited by Elsevier.
More details about the GJK algorithm can be found in the original paper
## Configuration 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 comes in two flavours: *accurate* and *fast* (default). You can
change before compiling by editing the main 'lib\CMakeLists.txt' file How to cite openGJK
(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 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).
the CMake cache.
License
## Examples -------
This section presents three examples on how to use openGJK with C, C# and Matlab. This project is licensed undert the GNU General Public License v3.0.
All the examples have been tested both Linux and Windows; the former used `make` and `gcc`, openGJK: open-source Gilbert-Johnson-Keerthi algorithm
the latter using `Visual studio 2017` and its compiler. Only x64 systems have been tested. Copyright (C) Mattia Montanari 2018 - 2019
http://iel.eng.ox.ac.uk/?page_id=504
### C
This example illustrates how to include openGJK in an existing C
program.
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.
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:
```
│ CMakeLists.txt
│ README.md
│
├───doc
│ openGJKcustomfooter.html
│ openGJKcustomheader.html
│ openGJKcustomstyle.css
│ Doxyfile
│ oxfordLogo.jpg
│
├───example1_c
│ CMakeLists.txt
│ main.c
│ userP.dat
│ userQ.dat
│
├───example2_mex
│ main.m
│ runme.m
│
├───example3_csharp
│ main.cs
│
└───lib
│ CMakeLists.txt
│
├───ext
│ predicates.c
│ predicates.h
│
├───include
│ └───openGJK
│ openGJK.h
│
└───src
openGJK.c
```
## Where to go next?
A clear presentation of the GJK algorithm can be found in the
book by **Van der Bergen** *Collision Detection in Interactive 3D
Environments*, edited by Elsevier.
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.
## Licence
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
------------------------------------------------------------------------

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

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

@ -1,45 +1,78 @@
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
# ##### # # # # # ##### # # # #
# #### ##### ###### # # # # # # # # # #### ##### ###### # # # # # # # #
# # # # # # ## # # # # # # # # # # # # ## # # # # # #
# # # # # ##### # # # # #### # ### # # # # # # ##### # # # # #### # ### #
# # # ##### # # # # # # # # # # # # # # ##### # # # # # # # # # # #
# # # # # # ## # # # # # # # # # # # # # ## # # # # # # #
# #### # ###### # # ##### ##### # # # # #### # ###### # # ##### ##### # # #
# # # #
# This file is part of openGJK. # # This file is part of openGJK. #
# # # #
# openGJK is free software: you can redistribute it and/or modify # # openGJK is free software: you can redistribute it and/or modify #
# it under the terms of the GNU General Public License as published by # # it under the terms of the GNU General Public License as published by #
# the Free Software Foundation, either version 3 of the License, or # # the Free Software Foundation, either version 3 of the License, or #
# any later version. # # any later version. #
# # # #
# openGJK is distributed in the hope that it will be useful, # # openGJK is distributed in the hope that it will be useful, #
# but WITHOUT ANY WARRANTY; without even the implied warranty of # # but WITHOUT ANY WARRANTY; without even the implied warranty of #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The # # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The #
# GNU General Public License for more details. # # GNU General Public License for more details. #
# # # #
# You should have received a copy of the GNU General Public License # # 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 # # openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
# Copyright (C) Mattia Montanari 2018 - 2019 # # Copyright (C) Mattia Montanari 2018 - 2019 #
# http://iel.eng.ox.ac.uk/?page_id=504 # # http://iel.eng.ox.ac.uk/?page_id=504 #
# # # #
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
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 source file set(TEST_NAME ${PROJECT_NAME}_CTEST)
set(SOURCE_FILES main.c )
message( "[${PROJECT_NAME}] CMake setting ..")
# Create the executable
add_executable(demo ${SOURCE_FILES}) # Set source file
set(SOURCE_FILES main.c )
# Link to openGJK
target_link_libraries(demo openGJKlib ) # Create the executable
add_executable(demo ${SOURCE_FILES})
# Report
message( ".. executable DONE!") # 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}" )

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

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

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

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

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

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

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

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

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

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

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

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

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;
}