New sub-algorithm
Mattia Montanari
parent
095b9f49c9
commit
214b2c47d6
178
CMakeLists.txt
178
CMakeLists.txt
|
|
@ -1,94 +1,84 @@
|
|||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
# ##### # # # #
|
||||
# #### ##### ###### # # # # # # # #
|
||||
# # # # # # ## # # # # # #
|
||||
# # # # # ##### # # # # #### # ### #
|
||||
# # # ##### # # # # # # # # # # #
|
||||
# # # # # # ## # # # # # # #
|
||||
# #### # ###### # # ##### ##### # # #
|
||||
# #
|
||||
# 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 #
|
||||
# #
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(openGJK)
|
||||
|
||||
set(CMAKE_C_STANDARD 11)
|
||||
|
||||
message("[${CMAKE_PROJECT_NAME}] Welcome, please change user options if needed.")
|
||||
|
||||
# APPLY DEFAULT SETTINGS
|
||||
if(NOT CMAKE_BUILD_TYPE)
|
||||
message("[${CMAKE_PROJECT_NAME}] Use default CMAKE_BUILD_TYPE")
|
||||
set(CMAKE_BUILD_TYPE Release)
|
||||
endif()
|
||||
|
||||
|
||||
# PLATFORM-SPECIFIC SETTING
|
||||
if (UNIX)
|
||||
find_library(M_LIB m)
|
||||
set(CMAKE_C_FLAGS "-lm")
|
||||
set(CMAKE_CXX_FLAGS "-lm")
|
||||
else ()
|
||||
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
|
||||
endif ()
|
||||
|
||||
# COMPILER SETTING
|
||||
IF(CMAKE_BUILD_TYPE MATCHES Release)
|
||||
set(CMAKE_BUILD_TYPE Release)
|
||||
ELSEIF(CMAKE_BUILD_TYPE MATCHES Debug)
|
||||
set(CMAKE_BUILD_TYPE Debug)
|
||||
ENDIF()
|
||||
|
||||
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
|
||||
# using GCC
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra -finline-functions")
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "-g -DDEBUG")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3")
|
||||
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -finline-functions")
|
||||
set(CMAKE_C_FLAGS_DEBUG "-g -DDEBUG")
|
||||
set(CMAKE_C_FLAGS_RELEASE "-O3")
|
||||
|
||||
add_compile_options(-static-libgcc -static-libstdc++ )
|
||||
add_definitions(-DMT)
|
||||
elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
|
||||
# using Visual Studio C++
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4131 /wd4701 /wd4255 /wd4710 /wd4820 /wd4711 /wd5045")
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "-DDEBUG /D_DEBUG /MDd /Zi /Ob0 /Od /RTC1")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "/Ox")
|
||||
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /wd4131 /wd4701 /wd4255 /wd4710 /wd4820 /wd4711 /wd5045")
|
||||
set(CMAKE_C_FLAGS_DEBUG "-DDEBUG /D_DEBUG /MDd /Zi /Ob0 /Od /RTC1")
|
||||
set(CMAKE_C_FLAGS_RELEASE "/Ox")
|
||||
|
||||
set(CMAKE_SUPPRESS_REGENERATION true)
|
||||
|
||||
endif()
|
||||
|
||||
# DEBUG FLAGS
|
||||
IF(CMAKE_BUILD_TYPE MATCHES Debug)
|
||||
add_definitions(-DDEBUG)
|
||||
ENDIF()
|
||||
|
||||
# INCLUDE LIBRARY AND EXAMPLE DIR
|
||||
add_subdirectory(lib)
|
||||
add_subdirectory(example1_c)
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
# ##### # # # #
|
||||
# #### ##### ###### # # # # # # # #
|
||||
# # # # # # ## # # # # # #
|
||||
# # # # # ##### # # # # #### # ### #
|
||||
# # # ##### # # # # # # # # # # #
|
||||
# # # # # # ## # # # # # # #
|
||||
# #### # ###### # # ##### ##### # # #
|
||||
# #
|
||||
# This file is part of openGJK. #
|
||||
# #
|
||||
# openGJK is free software: you can redistribute it and/or modify #
|
||||
# it under the terms of the GNU General Public License as published by #
|
||||
# the Free Software Foundation, either version 3 of the License, or #
|
||||
# any later version. #
|
||||
# #
|
||||
# openGJK is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The #
|
||||
# GNU General Public License for more details. #
|
||||
# #
|
||||
# You should have received a copy of the GNU General Public License #
|
||||
# along with openGJK. If not, see <https://www.gnu.org/licenses/>. #
|
||||
# #
|
||||
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
|
||||
# Copyright (C) Mattia Montanari 2018 - 2019 #
|
||||
# http://iel.eng.ox.ac.uk/?page_id=504 #
|
||||
# #
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
|
||||
message( "[${PROJECT_NAME}] CMake setting ..")
|
||||
|
||||
message(STATUS "Version : " ${openGJK_VERSION} )
|
||||
message(STATUS "Build type : " ${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()
|
||||
|
||||
IF(BUILD_STATIC_LIB)
|
||||
message(STATUS "Library type: " Static )
|
||||
add_library(${PROJECT_NAME} STATIC ${SOURCE_FILES} ${SOURCE_HEADS})
|
||||
add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=FALSE)
|
||||
ELSE()
|
||||
message(STATUS "Library type: " Shared )
|
||||
add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${SOURCE_HEADS})
|
||||
add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=FALSE)
|
||||
ENDIF(BUILD_STATIC_LIB)
|
||||
|
||||
# PLATFORM-SPECIFIC SETTING
|
||||
if (UNIX)
|
||||
find_library(M_LIB m)
|
||||
set(CMAKE_C_FLAGS "-lm")
|
||||
else ()
|
||||
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
|
||||
endif ()
|
||||
|
||||
find_package(OpenMP)
|
||||
if (OPENMP_FOUND)
|
||||
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
|
||||
set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
|
||||
endif()
|
||||
|
||||
# ADD DEFAULT COMPILER FLAGS
|
||||
include(CompilerFlags)
|
||||
|
||||
# Link include file
|
||||
target_include_directories( ${PROJECT_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
|
||||
target_link_libraries(${PROJECT_NAME} ${CMOCKA_LIBRARY} OpenMP::OpenMP_C )
|
||||
|
||||
IF(USE_PREDICATES)
|
||||
# for adpative floating-point artim.
|
||||
target_include_directories( ${PROJECT_NAME}
|
||||
PUBLIC ${PROJECT_SOURCE_DIR}/ext
|
||||
)
|
||||
ENDIF()
|
||||
300
INSTALL.md
300
INSTALL.md
|
|
@ -1,151 +1,151 @@
|
|||
# How to compile openGJK
|
||||
|
||||
Using openGJK is very simple. This guide will help you getting started compiling and using openGJK.
|
||||
|
||||
## Requirements
|
||||
|
||||
### Common requirements
|
||||
|
||||
1. A C compiler
|
||||
2. [CMake](http://www.cmake.org) version 3.5 or above
|
||||
|
||||
## Building
|
||||
First, you need to configure the compilation, using CMake.
|
||||
|
||||
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" ..`.
|
||||
|
||||
### CMake standard options
|
||||
|
||||
- CMAKE_BUILD_TYPE: The type of build (can be Debug or Release)
|
||||
- CMAKE_C_COMPILER: The path to the C compiler
|
||||
|
||||
### CMake options defined for openGJK
|
||||
|
||||
Options are defined in the following files:
|
||||
|
||||
- CmakeOptions.cmake
|
||||
|
||||
They can be changed with the -D option:
|
||||
|
||||
`cmake -DVERSION_ACCURATE=ON ..`
|
||||
|
||||
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
|
||||
and MacOS X).
|
||||
|
||||
- Go to the build dir
|
||||
- On Windows: run `cmakesetup`
|
||||
- On GNU/Linux and MacOS X: run `ccmake ..`
|
||||
|
||||
### Install and run
|
||||
|
||||
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/*`.
|
||||
|
||||
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
|
||||
|
||||
TO REWRITE!!
|
||||
|
||||
As mention above you can turn on the unit tests and make it possible to easily
|
||||
execute them:
|
||||
|
||||
`cmake -DCMAKE_BUILD_TYPE=Debug -DUNIT_TESTING=ON ..`
|
||||
|
||||
After that you can simply call `make test` in the build directory or if you
|
||||
want more output simply call `ctest -V`.
|
||||
|
||||
If you want to enable the generation of coverage files you can do this by
|
||||
using the following options:
|
||||
|
||||
`cmake -DCMAKE_BUILD_TYPE=Profiling -DUNIT_TESTING=ON ..`
|
||||
|
||||
After building it you will see that you have several coverage options in
|
||||
|
||||
`make help`
|
||||
|
||||
You should have `make ExperimentalCoverage` and running it will create
|
||||
coverage files. The result is stored in Testing directory.
|
||||
|
||||
## Examples
|
||||
|
||||
|
||||
This section presents three examples on how to use openGJK with C, C# and Matlab.
|
||||
|
||||
### 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. 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
|
||||
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#. 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
|
||||
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`
|
||||
|
||||
## API user reference
|
||||
|
||||
```double gjk( struct bodyA, struct bodyB, struct simplex)```
|
||||
|
||||
### Documentation
|
||||
The folder `doc` contains a Doxygen file for generating the documentation of the whole
|
||||
library. To build the documentation cd into `doc` and call Doxygen from the command line simply by typing `doxygen`. If correctly installed, Doxygen will create html documentation with graphs illustrating the call stack of the functions of the library.
|
||||
|
||||
### Parameters
|
||||
* **bodyA** The first body.
|
||||
* **bodyB** The second body.
|
||||
* **simplex** The simplex used the GJK algorithm at the first iteration.
|
||||
|
||||
### Returns
|
||||
* **double** the minimum distance between bodyA and bodyB.
|
||||
|
||||
### Description
|
||||
The function `gjk` computes the minimum Euclidean distance between two bodies using the
|
||||
GJK algorithm. Note that the simplex used at the first iteration may be initialised by the user, but this is not necessary.
|
||||
# How to compile openGJK
|
||||
|
||||
Using openGJK is very simple. This guide will help you getting started compiling and using openGJK.
|
||||
|
||||
## Requirements
|
||||
|
||||
### Common requirements
|
||||
|
||||
1. A C compiler
|
||||
2. [CMake](http://www.cmake.org) version 3.5 or above
|
||||
|
||||
## Building
|
||||
First, you need to configure the compilation, using CMake.
|
||||
|
||||
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" ..`.
|
||||
|
||||
### CMake standard options
|
||||
|
||||
- CMAKE_BUILD_TYPE: The type of build (can be Debug or Release)
|
||||
- CMAKE_C_COMPILER: The path to the C compiler
|
||||
|
||||
### CMake options defined for openGJK
|
||||
|
||||
Options are defined in the following files:
|
||||
|
||||
- CmakeOptions.cmake
|
||||
|
||||
They can be changed with the -D option:
|
||||
|
||||
`cmake -DVERSION_ACCURATE=ON ..`
|
||||
|
||||
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
|
||||
and MacOS X).
|
||||
|
||||
- Go to the build dir
|
||||
- On Windows: run `cmakesetup`
|
||||
- On GNU/Linux and MacOS X: run `ccmake ..`
|
||||
|
||||
### Install and run
|
||||
|
||||
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/*`.
|
||||
|
||||
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
|
||||
|
||||
TO REWRITE!!
|
||||
|
||||
As mention above you can turn on the unit tests and make it possible to easily
|
||||
execute them:
|
||||
|
||||
`cmake -DCMAKE_BUILD_TYPE=Debug -DUNIT_TESTING=ON ..`
|
||||
|
||||
After that you can simply call `make test` in the build directory or if you
|
||||
want more output simply call `ctest -V`.
|
||||
|
||||
If you want to enable the generation of coverage files you can do this by
|
||||
using the following options:
|
||||
|
||||
`cmake -DCMAKE_BUILD_TYPE=Profiling -DUNIT_TESTING=ON ..`
|
||||
|
||||
After building it you will see that you have several coverage options in
|
||||
|
||||
`make help`
|
||||
|
||||
You should have `make ExperimentalCoverage` and running it will create
|
||||
coverage files. The result is stored in Testing directory.
|
||||
|
||||
## Examples
|
||||
|
||||
|
||||
This section presents three examples on how to use openGJK with C, C# and Matlab.
|
||||
|
||||
### 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. 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
|
||||
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#. 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
|
||||
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`
|
||||
|
||||
## API user reference
|
||||
|
||||
```double gjk( struct bodyA, struct bodyB, struct simplex)```
|
||||
|
||||
### Documentation
|
||||
The folder `doc` contains a Doxygen file for generating the documentation of the whole
|
||||
library. To build the documentation cd into `doc` and call Doxygen from the command line simply by typing `doxygen`. If correctly installed, Doxygen will create html documentation with graphs illustrating the call stack of the functions of the library.
|
||||
|
||||
### Parameters
|
||||
* **bodyA** The first body.
|
||||
* **bodyB** The second body.
|
||||
* **simplex** The simplex used the GJK algorithm at the first iteration.
|
||||
|
||||
### Returns
|
||||
* **double** the minimum distance between bodyA and bodyB.
|
||||
|
||||
### Description
|
||||
The function `gjk` computes the minimum Euclidean distance between two bodies using the
|
||||
GJK algorithm. Note that the simplex used at the first iteration may be initialised by the user, but this is not necessary.
|
||||
|
||||
325
README.md
325
README.md
|
|
@ -1,230 +1,95 @@
|
|||
|
||||
openGJK {#mainpage}
|
||||
=======
|
||||
|
||||
The openGJK library uses the Gilbert-Johnson-Keerthi (GJK) algorithm to
|
||||
compute the minimum distance between convex polytopes. The
|
||||
implementation follows the description presented in
|
||||
"[Improving the GJK Algorithm for Faster and More Reliable Distance
|
||||
Queries Between Convex Objects. ACM Trans. on Graph. 2017](https://dl.acm.org/citation.cfm?id=3083724)" and has been tested
|
||||
on Unix and Windows systems for C, C# and Matlab programs.
|
||||
|
||||
This library offers researchers a tool that works
|
||||
out of the box: you can import it in your program and use it to measure
|
||||
the distance between two convex polytopes in 3D. All it needs are the
|
||||
coordinates of the vertices describing the two bodies.
|
||||
This library is not optimised for production, but it does provide a comprehensive and robust implementation. It is sufficiently fast for
|
||||
most applications, and you can also build from here to suite your own
|
||||
application. For instance, openGJK is not for incremental and is not
|
||||
for NURBS, but it offers a good starting point for such specific
|
||||
applications.
|
||||
|
||||
## Getting Started
|
||||
Using openGJK is very simple. This guide will help you getting
|
||||
started compiling and using openGJK.
|
||||
|
||||
### When should I use openGJK?
|
||||
|
||||
OpenGJK is designed with accuracy and robustness in mind and is
|
||||
suitable for engineering simulations. Good use of this library
|
||||
include the finite element method (FEM) and discrete element method (DEM).
|
||||
|
||||
Basically, openGJK can measure the distance between **any convex polytope**. For example:
|
||||
- line segments
|
||||
- triangles
|
||||
- tetrahedrons
|
||||
- cubes.
|
||||
|
||||
### Installing the openGJK library
|
||||
|
||||
#### Prerequisites
|
||||
|
||||
1. A compiler (gnu or Microsoft Visual Studio for C)
|
||||
2. CMake version 3.5 or above
|
||||
3. Only for the Matlab interface you will need to build mex files (find out the requisites from [Mathworks documentation](https://uk.mathworks.com/help/matlab/matlab_external/what-you-need-to-build-mex-files.html)).
|
||||
4. Only for the C# interface on Unix you will need [mono](http://www.mono-project.com/) and Microsoft Visual Studio toolchain for C# on Windows.
|
||||
|
||||
### Installation
|
||||
|
||||
There are CMake files for compiling openGJK in the usual
|
||||
way:
|
||||
1. Create a new folder in the folder containing this readme file.
|
||||
2. Move into that folder and type `cmake -G <duild-system> ..`. For example,
|
||||
on Windows you can type `cmake -G "Visual Studio 15 2017 Win64" ..`, on Unix `cmake -G "Unix Makefiles" ..`.
|
||||
3. Use the files generated by Cmake to build the library. Whether you compile
|
||||
with `make` or an IDE, you will build a shared library and an executable
|
||||
for the C example. For Matlab and C# applications, see sections below.
|
||||
|
||||
To install the library you should copy the header file openGJK.h and the binaries in a folder accessible in the search path by all users (on Unix this would normally be /usr/local).
|
||||
|
||||
### Automated documentation
|
||||
The folder `doc` contains a Doxygen file for generating the documentation of the whole
|
||||
library. To build the documentation cd into `doc` and call Doxygen from the command line simply by typing `doxygen`. If correctly installed, Doxygen will create html documentation with graphs illustrating the call stack of the functions of the library.
|
||||
|
||||
## API user reference
|
||||
|
||||
```double gjk( struct bodyA, struct bodyB, struct simplex)```
|
||||
|
||||
### Parameters
|
||||
* **bodyA** The first body.
|
||||
* **bodyB** The second body.
|
||||
* **simplex** The simplex used the GJK algorithm at the first iteration.
|
||||
|
||||
### Returns
|
||||
* **double** the minimum distance between bodyA and bodyB.
|
||||
|
||||
### Description
|
||||
The function `gjk` computes the minimum Euclidean distance between two bodies using the
|
||||
GJK algorithm. Note that the simplex used at the first iteration may be initialised by the user, but this is not necessary.
|
||||
|
||||
|
||||
## Configuration
|
||||
|
||||
openGJK comes in two flavours: *accurate* and *fast* (default). You can
|
||||
change before compiling by editing the main 'lib\CMakeLists.txt' file
|
||||
(in the folder `lib`). Set the option `VERSION_ACCURATE` to `ON` and
|
||||
run CMake. You can verify what version is being compiled from the terminal,
|
||||
if you do not see "Version: Accurate" when calling CMake, you have to clean
|
||||
the CMake cache.
|
||||
|
||||
## Examples
|
||||
|
||||
This section presents three examples on how to use openGJK with C, C# and Matlab.
|
||||
All the examples have been tested both Linux and Windows; the former used `make` and `gcc`,
|
||||
the latter using `Visual studio 2017` and its compiler. Only x64 systems have been tested.
|
||||
|
||||
### 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
|
||||
------------------------------------------------------------------------
|
||||
|
||||
openGJK {#mainpage}
|
||||
=======
|
||||
|
||||
OpenGJK implements a new version of the Gilbert-Johnson-Keerthi (GJK) algorithm to
|
||||
compute the minimum distance between convex polytopes. OpenGJK is a C library which was tested on Unix and Windows using different compilers for multi-threaded applications.
|
||||
|
||||
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)".
|
||||
|
||||
|
||||
When should I use openGJK?
|
||||
--------------------------
|
||||
|
||||
OpenGJK is designed with speed, accuracy and robustness in mind and is therefore suitable for engineering, robotics and computer graphics simulations.
|
||||
Basically, openGJK can be used in any application where the distance between **any convex polytope** is required.
|
||||
|
||||
Compile and run
|
||||
---------------
|
||||
|
||||
To compile the OpenGJK library create a build dir,
|
||||
and in the build dir call 'cmake ..' followed by 'make'. More details can be found in the INSTALL file.
|
||||
|
||||
There are examples for C, C# and Matlab in the `examples` folder. The INSTALL file provides information on how to run the examples.
|
||||
|
||||
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
|
||||
```
|
||||
|
||||
More information
|
||||
----------------
|
||||
|
||||
[OpenGJK](http://iel.eng.ox.ac.uk/?page_id=504) was developed at the Impact Engineering Laboratory, University of Oxford.
|
||||
|
||||
|
||||
A clear presentation of the GJK algorithm can be found in the
|
||||
book by **Van der Bergen** *Collision Detection in Interactive 3D
|
||||
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).
|
||||
|
||||
|
||||
How to cite openGJK
|
||||
-------------------
|
||||
|
||||
If you use openGJK for your research please cite [OpenGJK for C, C# and Matlab: Reliable solutions to distance queries between convex bodies in three-dimensional space. SoftwareX. 2018](https://www.sciencedirect.com/science/article/pii/S2352711018300591).
|
||||
|
||||
License
|
||||
-------
|
||||
|
||||
This project is licensed undert the GNU General Public License v3.0.
|
||||
openGJK: open-source Gilbert-Johnson-Keerthi algorithm
|
||||
Copyright (C) Mattia Montanari 2018 - 2019
|
||||
http://iel.eng.ox.ac.uk/?page_id=504
|
||||
2491
doc/Doxyfile
2491
doc/Doxyfile
File diff suppressed because it is too large
Load Diff
|
|
@ -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  <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>
|
||||
|
|
@ -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
|
||||
 <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 -->
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
Before Width: | Height: | Size: 29 KiB |
|
|
@ -1,45 +1,78 @@
|
|||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
# ##### # # # #
|
||||
# #### ##### ###### # # # # # # # #
|
||||
# # # # # # ## # # # # # #
|
||||
# # # # # ##### # # # # #### # ### #
|
||||
# # # ##### # # # # # # # # # # #
|
||||
# # # # # # ## # # # # # # #
|
||||
# #### # ###### # # ##### ##### # # #
|
||||
# #
|
||||
# This file is part of openGJK. #
|
||||
# #
|
||||
# openGJK is free software: you can redistribute it and/or modify #
|
||||
# it under the terms of the GNU General Public License as published by #
|
||||
# the Free Software Foundation, either version 3 of the License, or #
|
||||
# any later version. #
|
||||
# #
|
||||
# openGJK is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The #
|
||||
# GNU General Public License for more details. #
|
||||
# #
|
||||
# You should have received a copy of the GNU General Public License #
|
||||
# along with Foobar. If not, see <https://www.gnu.org/licenses/>. #
|
||||
# #
|
||||
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
|
||||
# Copyright (C) Mattia Montanari 2018 - 2019 #
|
||||
# http://iel.eng.ox.ac.uk/?page_id=504 #
|
||||
# #
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
|
||||
project(openGJKdemo)
|
||||
|
||||
message( "[${CMAKE_PROJECT_NAME}] Compiling the executable ..")
|
||||
|
||||
# Set source file
|
||||
set(SOURCE_FILES main.c )
|
||||
|
||||
# Create the executable
|
||||
add_executable(demo ${SOURCE_FILES})
|
||||
|
||||
# Link to openGJK
|
||||
target_link_libraries(demo openGJKlib )
|
||||
|
||||
# Report
|
||||
message( ".. executable DONE!")
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
# ##### # # # #
|
||||
# #### ##### ###### # # # # # # # #
|
||||
# # # # # # ## # # # # # #
|
||||
# # # # # ##### # # # # #### # ### #
|
||||
# # # ##### # # # # # # # # # # #
|
||||
# # # # # # ## # # # # # # #
|
||||
# #### # ###### # # ##### ##### # # #
|
||||
# #
|
||||
# This file is part of openGJK. #
|
||||
# #
|
||||
# openGJK is free software: you can redistribute it and/or modify #
|
||||
# it under the terms of the GNU General Public License as published by #
|
||||
# the Free Software Foundation, either version 3 of the License, or #
|
||||
# any later version. #
|
||||
# #
|
||||
# openGJK is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The #
|
||||
# GNU General Public License for more details. #
|
||||
# #
|
||||
# You should have received a copy of the GNU General Public License #
|
||||
# along with openGJK. If not, see <https://www.gnu.org/licenses/>. #
|
||||
# #
|
||||
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
|
||||
# Copyright (C) Mattia Montanari 2018 - 2019 #
|
||||
# http://iel.eng.ox.ac.uk/?page_id=504 #
|
||||
# #
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
|
||||
project(openGJKdemo VERSION 1.0.0 LANGUAGES C)
|
||||
|
||||
set(APPLICATION_NAME ${PROJECT_NAME})
|
||||
set(CMAKE_C_STANDARD 11)
|
||||
set(TEST_NAME ${PROJECT_NAME}_CTEST)
|
||||
|
||||
message( "[${PROJECT_NAME}] CMake setting ..")
|
||||
|
||||
# Set source file
|
||||
set(SOURCE_FILES main.c )
|
||||
|
||||
# Create the executable
|
||||
add_executable(demo ${SOURCE_FILES})
|
||||
|
||||
# Copy input files after build
|
||||
add_custom_command(
|
||||
TARGET demo POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND} -E copy
|
||||
${CMAKE_SOURCE_DIR}/examples/c/userP.dat
|
||||
${CMAKE_CURRENT_BINARY_DIR}/userP.dat
|
||||
COMMAND ${CMAKE_COMMAND} -E copy
|
||||
${CMAKE_SOURCE_DIR}/examples/c/userQ.dat
|
||||
${CMAKE_CURRENT_BINARY_DIR}/userQ.dat)
|
||||
|
||||
# PLATFORM-SPECIFIC SETTING
|
||||
if (UNIX)
|
||||
find_library(M_LIB m)
|
||||
# Link to openGJK and math library
|
||||
target_link_libraries(demo openGJKlib m)
|
||||
else ()
|
||||
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
|
||||
target_link_libraries(demo openGJKlib)
|
||||
endif ()
|
||||
|
||||
|
||||
include(AddCMockaTest)
|
||||
find_library(LIB_CMOCKA cmocka)
|
||||
target_link_libraries(demo ${CMOCKA_LIBRARY})
|
||||
if(NOT LIB_CMOCKA)
|
||||
message(STATUS "${LIB_CMOCKA} library not found ")
|
||||
endif()
|
||||
|
||||
if (UNIT_TESTING)
|
||||
target_link_libraries(demo cmocka)
|
||||
add_test( ${TEST_NAME} demo)
|
||||
endif (UNIT_TESTING)
|
||||
|
||||
message(STATUS "Completed CMake setting for ${PROJECT_NAME}" )
|
||||
|
|
@ -1,177 +1,178 @@
|
|||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* This file is part of openGJK. *
|
||||
* *
|
||||
* openGJK is free software: you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation, either version 3 of the License, or *
|
||||
* any later version. *
|
||||
* *
|
||||
* openGJK is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
|
||||
* GNU General Public License for more details. *
|
||||
* *
|
||||
* You should have received a copy of the GNU General Public License *
|
||||
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
|
||||
* *
|
||||
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
|
||||
* Copyright (C) Mattia Montanari 2018 - 2019 *
|
||||
* http://iel.eng.ox.ac.uk/?page_id=504 *
|
||||
* *
|
||||
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* *
|
||||
* This file runs an example to illustrate how to invoke the openGJK lib *
|
||||
* within a C program. An executable called 'demo' can be compiled with *
|
||||
* CMake. This reads the coordinates of two polytopes from the input *
|
||||
* files userP.dat and userQ.dat, respectively, and returns the minimum *
|
||||
* distance between them computed using the openGJK library. *
|
||||
* *
|
||||
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
|
||||
|
||||
/**
|
||||
* @file main.c
|
||||
* @author Mattia Montanari
|
||||
* @date April 2018
|
||||
* @brief File illustrating an application that invokes openGJK.
|
||||
*
|
||||
*/
|
||||
|
||||
#define _CRT_HAS_CXX17 0
|
||||
#include <stdio.h>
|
||||
|
||||
/* For importing openGJK this is Step 1: include header in subfolder. */
|
||||
#include "openGJK/openGJK.h"
|
||||
|
||||
#ifndef WIN32
|
||||
#define fscanf_s fscanf
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Function for reading input file with body's coordinates.
|
||||
*
|
||||
*/
|
||||
int readinput ( const char *inputfile, double ***pts, int * out ) {
|
||||
int npoints = 0;
|
||||
int idx = 0;
|
||||
FILE *fp;
|
||||
|
||||
/* Open file. */
|
||||
#ifdef WIN32
|
||||
errno_t err;
|
||||
if ((err = fopen_s(&fp, inputfile, "r")) != 0) {
|
||||
#else
|
||||
if ((fp = fopen(inputfile, "r")) == NULL) {
|
||||
#endif
|
||||
fprintf(stdout, "ERROR: input file %s not found!\n", inputfile);
|
||||
fprintf(stdout, " -> The file must be in the folder from which this program is launched\n\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* Read number of input vertices. */
|
||||
if (fscanf_s(fp, "%d", &npoints) != 1)
|
||||
return 1;
|
||||
|
||||
/* Allocate memory. */
|
||||
double **arr = (double **)malloc(npoints * sizeof(double *));
|
||||
for (int i=0; i<npoints; i++)
|
||||
arr[i] = (double *)malloc(3 * sizeof(double));
|
||||
|
||||
/* Read and store vertices' coordinates. */
|
||||
for (idx = 0; idx < npoints; idx++)
|
||||
{
|
||||
if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3 )
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* Close file. */
|
||||
fclose(fp);
|
||||
|
||||
/* Pass pointers. */
|
||||
*pts = arr;
|
||||
*out = idx;
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Main program of example1_c (described in Section 3.1 of the paper).
|
||||
*
|
||||
*/
|
||||
int main() {
|
||||
/* Squared distance computed by openGJK. */
|
||||
double dd;
|
||||
/* Structure of simplex used by openGJK. */
|
||||
struct simplex s;
|
||||
/* Number of vertices defining body 1 and body 2, respectively. */
|
||||
int nvrtx1,
|
||||
nvrtx2;
|
||||
/* Structures of body 1 and body 2, respectively. */
|
||||
struct bd bd1;
|
||||
struct bd bd2;
|
||||
/* Specify name of input files for body 1 and body 2, respectively. */
|
||||
char inputfileA[40] = "userP.dat",
|
||||
inputfileB[40] = "userQ.dat";
|
||||
/* Pointers to vertices' coordinates of body 1 and body 2, respectively. */
|
||||
double (**vrtx1) = NULL,
|
||||
(**vrtx2) = NULL;
|
||||
|
||||
/* For importing openGJK this is Step 2: adapt the data structure for the
|
||||
* two bodies that will be passed to the GJK procedure. */
|
||||
|
||||
/* Import coordinates of object 1. */
|
||||
if (readinput ( inputfileA, &vrtx1, &nvrtx1 ))
|
||||
return (1);
|
||||
bd1.coord = vrtx1;
|
||||
bd1.numpoints = nvrtx1;
|
||||
|
||||
/* Import coordinates of object 2. */
|
||||
if (readinput ( inputfileB, &vrtx2, &nvrtx2 ))
|
||||
return (1);
|
||||
bd2.coord = vrtx2;
|
||||
bd2.numpoints = nvrtx2;
|
||||
|
||||
/* Initialise simplex as empty */
|
||||
s.nvrtx = 0;
|
||||
|
||||
#ifdef DEBUG
|
||||
/* Verify input of body A. */
|
||||
for (int i = 0; i < bd1.numpoints; ++i) {
|
||||
printf ( "%.2f ", vrtx1[i][0]);
|
||||
printf ( "%.2f ", vrtx1[i][1]);
|
||||
printf ( "%.2f\n", bd1.coord[i][2]);
|
||||
}
|
||||
|
||||
/* Verify input of body B. */
|
||||
for (int i = 0; i < bd2.numpoints; ++i) {
|
||||
printf ( "%.2f ", bd2.coord[i][0]);
|
||||
printf ( "%.2f ", bd2.coord[i][1]);
|
||||
printf ( "%.2f\n", bd2.coord[i][2]);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* For importing openGJK this is Step 3: invoke the GJK procedure. */
|
||||
/* Compute squared distance using GJK algorithm. */
|
||||
dd = gjk (bd1, bd2, &s);
|
||||
|
||||
/* Print distance between objects. */
|
||||
printf ("Distance between bodies %f\n", dd);
|
||||
|
||||
/* Free memory */
|
||||
for (int i=0; i<bd1.numpoints; i++)
|
||||
free(bd1.coord[i]);
|
||||
free(bd1.coord);
|
||||
for (int i=0; i<bd2.numpoints; i++)
|
||||
free(bd2.coord[i]);
|
||||
free(bd2.coord);
|
||||
|
||||
return (0);
|
||||
}
|
||||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* This file is part of openGJK. *
|
||||
* *
|
||||
* openGJK is free software: you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation, either version 3 of the License, or *
|
||||
* any later version. *
|
||||
* *
|
||||
* openGJK is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
|
||||
* GNU General Public License for more details. *
|
||||
* *
|
||||
* You should have received a copy of the GNU General Public License *
|
||||
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
|
||||
* *
|
||||
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
|
||||
* Copyright (C) Mattia Montanari 2018 - 2019 *
|
||||
* http://iel.eng.ox.ac.uk/?page_id=504 *
|
||||
* *
|
||||
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* *
|
||||
* This file runs an example to illustrate how to invoke the openGJK lib *
|
||||
* within a C program. An executable called 'demo' can be compiled with *
|
||||
* CMake. This reads the coordinates of two polytopes from the input *
|
||||
* files userP.dat and userQ.dat, respectively, and returns the minimum *
|
||||
* distance between them computed using the openGJK library. *
|
||||
* *
|
||||
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
|
||||
|
||||
/**
|
||||
* @file main.c
|
||||
* @author Mattia Montanari
|
||||
* @date April 2018
|
||||
* @brief File illustrating an application that invokes openGJK.
|
||||
*
|
||||
*/
|
||||
|
||||
#define _CRT_HAS_CXX17 0
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
/* For importing openGJK this is Step 1: include header in subfolder. */
|
||||
#include "openGJK/openGJK.h"
|
||||
|
||||
#ifndef WIN32
|
||||
#define fscanf_s fscanf
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Function for reading input file with body's coordinates.
|
||||
*
|
||||
*/
|
||||
int readinput(const char *inputfile, double ***pts, int * out) {
|
||||
int npoints = 0;
|
||||
int idx = 0;
|
||||
FILE *fp;
|
||||
|
||||
/* Open file. */
|
||||
#ifdef WIN32
|
||||
errno_t err;
|
||||
if ((err = fopen_s(&fp, inputfile, "r")) != 0) {
|
||||
#else
|
||||
if ((fp = fopen(inputfile, "r")) == NULL) {
|
||||
#endif
|
||||
fprintf(stdout, "ERROR: input file %s not found!\n", inputfile);
|
||||
fprintf(stdout, " -> The file must be in the folder from which this program is launched\n\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* Read number of input vertices. */
|
||||
if (fscanf_s(fp, "%d", &npoints) != 1)
|
||||
return 1;
|
||||
|
||||
/* Allocate memory. */
|
||||
double **arr = (double **)malloc(npoints * sizeof(double *));
|
||||
for (int i = 0; i < npoints; i++)
|
||||
arr[i] = (double *)malloc(3 * sizeof(double));
|
||||
|
||||
/* Read and store vertices' coordinates. */
|
||||
for (idx = 0; idx < npoints; idx++)
|
||||
{
|
||||
if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3)
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* Close file. */
|
||||
fclose(fp);
|
||||
|
||||
/* Pass pointers. */
|
||||
*pts = arr;
|
||||
*out = idx;
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Main program of example1_c (described in Section 3.1 of the paper).
|
||||
*
|
||||
*/
|
||||
int main() {
|
||||
/* Squared distance computed by openGJK. */
|
||||
double dd;
|
||||
/* Structure of simplex used by openGJK. */
|
||||
struct simplex s;
|
||||
/* Number of vertices defining body 1 and body 2, respectively. */
|
||||
int nvrtx1,
|
||||
nvrtx2;
|
||||
/* Structures of body 1 and body 2, respectively. */
|
||||
struct bd bd1;
|
||||
struct bd bd2;
|
||||
/* Specify name of input files for body 1 and body 2, respectively. */
|
||||
char inputfileA[40] = "userP.dat",
|
||||
inputfileB[40] = "userQ.dat";
|
||||
/* Pointers to vertices' coordinates of body 1 and body 2, respectively. */
|
||||
double(**vrtx1) = NULL,
|
||||
(**vrtx2) = NULL;
|
||||
|
||||
/* For importing openGJK this is Step 2: adapt the data structure for the
|
||||
* two bodies that will be passed to the GJK procedure. */
|
||||
|
||||
/* Import coordinates of object 1. */
|
||||
if (readinput(inputfileA, &vrtx1, &nvrtx1))
|
||||
return (1);
|
||||
bd1.coord = vrtx1;
|
||||
bd1.numpoints = nvrtx1;
|
||||
|
||||
/* Import coordinates of object 2. */
|
||||
if (readinput(inputfileB, &vrtx2, &nvrtx2))
|
||||
return (1);
|
||||
bd2.coord = vrtx2;
|
||||
bd2.numpoints = nvrtx2;
|
||||
|
||||
/* Initialise simplex as empty */
|
||||
s.nvrtx = 0;
|
||||
|
||||
#ifdef DEBUG
|
||||
/* Verify input of body A. */
|
||||
for (int i = 0; i < bd1.numpoints; ++i) {
|
||||
printf("%.2f ", vrtx1[i][0]);
|
||||
printf("%.2f ", vrtx1[i][1]);
|
||||
printf("%.2f\n", bd1.coord[i][2]);
|
||||
}
|
||||
|
||||
/* Verify input of body B. */
|
||||
for (int i = 0; i < bd2.numpoints; ++i) {
|
||||
printf("%.2f ", bd2.coord[i][0]);
|
||||
printf("%.2f ", bd2.coord[i][1]);
|
||||
printf("%.2f\n", bd2.coord[i][2]);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* For importing openGJK this is Step 3: invoke the GJK procedure. */
|
||||
/* Compute squared distance using GJK algorithm. */
|
||||
dd = gjk(bd1, bd2, &s);
|
||||
|
||||
/* Print distance between objects. */
|
||||
printf("Distance between bodies %f\n", dd);
|
||||
|
||||
/* Free memory */
|
||||
for (int i = 0; i < bd1.numpoints; i++)
|
||||
free(bd1.coord[i]);
|
||||
free(bd1.coord);
|
||||
for (int i = 0; i < bd2.numpoints; i++)
|
||||
free(bd2.coord[i]);
|
||||
free(bd2.coord);
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
|
@ -1,10 +1,10 @@
|
|||
9
|
||||
0.0 5.5 0.0
|
||||
2.3 1.0 -2.0
|
||||
8.1 4.0 2.4
|
||||
4.3 5.0 2.2
|
||||
2.5 1.0 2.3
|
||||
7.1 1.0 2.4
|
||||
1.0 1.5 0.3
|
||||
3.3 0.5 0.3
|
||||
6.0 1.4 0.2
|
||||
9
|
||||
0.0 5.5 0.0
|
||||
2.3 1.0 -2.0
|
||||
8.1 4.0 2.4
|
||||
4.3 5.0 2.2
|
||||
2.5 1.0 2.3
|
||||
7.1 1.0 2.4
|
||||
1.0 1.5 0.3
|
||||
3.3 0.5 0.3
|
||||
6.0 1.4 0.2
|
||||
|
|
@ -1,10 +1,10 @@
|
|||
9
|
||||
-0.0 -5.5 0.0
|
||||
-2.3 -1.0 2.0
|
||||
-8.1 -4.0 -2.4
|
||||
-4.3 -5.0 -2.2
|
||||
-2.5 -1.0 -2.3
|
||||
-7.1 -1.0 -2.4
|
||||
-1.0 -1.5 -0.3
|
||||
-3.3 -0.5 -0.3
|
||||
-6.0 -1.4 -0.2
|
||||
9
|
||||
-0.0 -5.5 0.0
|
||||
-2.3 -1.0 2.0
|
||||
-8.1 -4.0 -2.4
|
||||
-4.3 -5.0 -2.2
|
||||
-2.5 -1.0 -2.3
|
||||
-7.1 -1.0 -2.4
|
||||
-1.0 -1.5 -0.3
|
||||
-3.3 -0.5 -0.3
|
||||
-6.0 -1.4 -0.2
|
||||
|
|
@ -1,67 +1,67 @@
|
|||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* This file is part of openGJK. *
|
||||
* *
|
||||
* openGJK is free software: you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation, either version 3 of the License, or *
|
||||
* any later version. *
|
||||
* *
|
||||
* openGJK is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
|
||||
* GNU General Public License for more details. *
|
||||
* *
|
||||
* You should have received a copy of the GNU General Public License *
|
||||
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
|
||||
* *
|
||||
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
|
||||
* Copyright (C) Mattia Montanari 2018 - 2019 *
|
||||
* http://iel.eng.ox.ac.uk/?page_id=504 *
|
||||
* *
|
||||
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
|
||||
|
||||
using System;
|
||||
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
public class Tester
|
||||
{
|
||||
|
||||
|
||||
#if UNIX
|
||||
[DllImport("libopenGJKlib.so", EntryPoint="csFunction", CallingConvention = CallingConvention.StdCall)]
|
||||
#else
|
||||
[DllImport("openGJKlib", EntryPoint = "csFunction", CallingConvention = CallingConvention.StdCall)]
|
||||
#endif
|
||||
static extern double gjk(int na, double [,] ia, int nb, double [,] ib);
|
||||
|
||||
public static void Main(string[] args)
|
||||
{
|
||||
double dist;
|
||||
// Define array A with coordinates
|
||||
int nCoordsA = 9;
|
||||
var inCoordsA = new double[3,9] { {0.0 , 2.3 , 8.1 , 4.3 ,2.5 , 7.1 , 1.0 , 3.3 , 6.0} , { 5.5 , 1.0 , 4.0 , 5.0 ,1.0, 1.0, 1.5, 0.5 , 1.4} ,{ 0.0 , -2.0, 2.4, 2.2, 2.3 , 2.4 , 0.3 , 0.3 , 0.2} };
|
||||
|
||||
// Define array B with coordinates
|
||||
int nCoordsB = 9;
|
||||
var inCoordsB = new double[3,9] { {-0.0 , -2.3 , -8.1 , -4.3 ,-2.5 , -7.1 , -1.0 , -3.3 , -6.0} , { -5.5 , -1.0 ,- 4.0 ,- 5.0 ,-1.0, -1.0, -1.5, -0.5 , -1.4} ,{ -0.0 , 2.0, -2.4, -2.2, -2.3 , -2.4 , -0.3 , -0.3 , -0.2} };
|
||||
|
||||
// Invoke GJK to compute distance
|
||||
dist = gjk( nCoordsA, inCoordsA, nCoordsB, inCoordsB );
|
||||
|
||||
// Output results
|
||||
var s = string.Format("{0:0.##}", dist);
|
||||
var message = string.Format("The distance between {0} is {1}","A and B",s);
|
||||
Console.WriteLine(message);
|
||||
Console.WriteLine("Press any key to exit");
|
||||
Console.ReadLine();
|
||||
}
|
||||
}
|
||||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* This file is part of openGJK. *
|
||||
* *
|
||||
* openGJK is free software: you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation, either version 3 of the License, or *
|
||||
* any later version. *
|
||||
* *
|
||||
* openGJK is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
|
||||
* GNU General Public License for more details. *
|
||||
* *
|
||||
* You should have received a copy of the GNU General Public License *
|
||||
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
|
||||
* *
|
||||
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
|
||||
* Copyright (C) Mattia Montanari 2018 - 2019 *
|
||||
* http://iel.eng.ox.ac.uk/?page_id=504 *
|
||||
* *
|
||||
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
|
||||
|
||||
using System;
|
||||
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
public class Tester
|
||||
{
|
||||
|
||||
|
||||
#if UNIX
|
||||
[DllImport("libopenGJKlib.so", EntryPoint="csFunction", CallingConvention = CallingConvention.StdCall)]
|
||||
#else
|
||||
[DllImport("openGJKlib", EntryPoint = "csFunction", CallingConvention = CallingConvention.StdCall)]
|
||||
#endif
|
||||
static extern double gjk(int na, double [,] ia, int nb, double [,] ib);
|
||||
|
||||
public static void Main(string[] args)
|
||||
{
|
||||
double dist;
|
||||
// Define array A with coordinates
|
||||
int nCoordsA = 9;
|
||||
var inCoordsA = new double[3,9] { {0.0 , 2.3 , 8.1 , 4.3 ,2.5 , 7.1 , 1.0 , 3.3 , 6.0} , { 5.5 , 1.0 , 4.0 , 5.0 ,1.0, 1.0, 1.5, 0.5 , 1.4} ,{ 0.0 , -2.0, 2.4, 2.2, 2.3 , 2.4 , 0.3 , 0.3 , 0.2} };
|
||||
|
||||
// Define array B with coordinates
|
||||
int nCoordsB = 9;
|
||||
var inCoordsB = new double[3,9] { {-0.0 , -2.3 , -8.1 , -4.3 ,-2.5 , -7.1 , -1.0 , -3.3 , -6.0} , { -5.5 , -1.0 ,- 4.0 ,- 5.0 ,-1.0, -1.0, -1.5, -0.5 , -1.4} ,{ -0.0 , 2.0, -2.4, -2.2, -2.3 , -2.4 , -0.3 , -0.3 , -0.2} };
|
||||
|
||||
// Invoke GJK to compute distance
|
||||
dist = gjk( nCoordsA, inCoordsA, nCoordsB, inCoordsB );
|
||||
|
||||
// Output results
|
||||
var s = string.Format("{0:0.##}", dist);
|
||||
var message = string.Format("The distance between {0} is {1}","A and B",s);
|
||||
Console.WriteLine(message);
|
||||
Console.WriteLine("Press any key to exit");
|
||||
Console.ReadLine();
|
||||
}
|
||||
}
|
||||
|
|
@ -1,78 +1,78 @@
|
|||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% ##### # # # %
|
||||
% #### ##### ###### # # # # # # # %
|
||||
% # # # # # ## # # # # # %
|
||||
% # # # # ##### # # # # #### # ### %
|
||||
% # # ##### # # # # # # # # # # %
|
||||
% # # # # # ## # # # # # # %
|
||||
% #### # ###### # # ##### ##### # # %
|
||||
% %
|
||||
% This file is part of openGJK. %
|
||||
% %
|
||||
% openGJK is free software: you can redistribute it and/or modify %
|
||||
% it under the terms of the GNU General Public License as published by %
|
||||
% the Free Software Foundation, either version 3 of the License, or %
|
||||
% any later version. %
|
||||
% %
|
||||
% openGJK is distributed in the hope that it will be useful, %
|
||||
% but WITHOUT ANY WARRANTY; without even the implied warranty of %
|
||||
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The %
|
||||
% GNU General Public License for more details. %
|
||||
% %
|
||||
% You should have received a copy of the GNU General Public License %
|
||||
% along with Foobar. If not, see <https://www.gnu.org/licenses/>. %
|
||||
% %
|
||||
% openGJK: open-source Gilbert-Johnson-Keerthi algorithm %
|
||||
% Copyright (C) Mattia Montanari 2018 - 2019 %
|
||||
% http://iel.eng.ox.ac.uk/?page_id=504 %
|
||||
% %
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% %
|
||||
% This file runs an example to illustrate how to cll the openGJK library %
|
||||
% withing Matlab. It assumes that a mex file openGJK is availalbe, see %
|
||||
% the runme.m script for information on how to compile it. %
|
||||
% The example computes the minimum distance between two polytopes in 3D, %
|
||||
% A and B, both defined as a list of points. %
|
||||
% %
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
|
||||
% DEFINE BODY A AS 3xN MATRIX, WHERE N IS THE NUMBER OF VERTICES OF BODY A
|
||||
A = [ 0.0 2.3 8.1 4.3 2.5 7.1 1.0 3.3 6.0
|
||||
5.5 1.0 4.0 5.0 1.0 1.0 1.5 0.5 1.4
|
||||
0.0 -2.0 2.4 2.2 2.3 2.4 0.3 0.3 0.2];
|
||||
|
||||
% DEFINE BODY B IN THE OPPOSITE QUADRANT OF BODY A
|
||||
B = -A;
|
||||
|
||||
% COMPUTE MINIMUM DISTANCE AND RETURN VALUE
|
||||
dist = openGJK( A, B );
|
||||
fprintf('The minimum distance between A and B is %.2f\n',dist);
|
||||
|
||||
% VISUALISE RESULTS
|
||||
% .. create new figure
|
||||
figure('units','centimeters', 'WindowStyle','normal', 'color','w',...
|
||||
'Position',[0 8.5 9 6],'defaultAxesColorOrder',parula,...
|
||||
'Renderer','opengl')
|
||||
% .. adjust properties
|
||||
axis equal tight off; hold all;
|
||||
% .. display body A
|
||||
DT = delaunayTriangulation(A');
|
||||
[K,~] = convexHull(DT);
|
||||
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
|
||||
'EdgeColor','none','FaceColor',[.4 1 .9 ],...
|
||||
'FaceLighting','flat' )
|
||||
% .. display body B
|
||||
DT = delaunayTriangulation(B');
|
||||
[K,~] = convexHull(DT);
|
||||
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
|
||||
'EdgeColor','none','FaceColor',[.4 1 .8 ],...
|
||||
'FaceLighting','flat' )
|
||||
% .. represent the computed distance as a sphere
|
||||
[x,y,z] = sphere(100);
|
||||
surf(x.*dist/2,y.*dist/2,z.*dist/2,'facecolor',[.9 .9 .9],...
|
||||
'EdgeColor','none','FaceLighting','flat','SpecularColorReflectance',0,...
|
||||
'SpecularStrength',1,'SpecularExponent',10,'facealpha',.7)
|
||||
% ... adjust point of view
|
||||
view(42,21)
|
||||
% ... add light
|
||||
light('Position',[5 -10 20],'Style','local');
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% ##### # # # %
|
||||
% #### ##### ###### # # # # # # # %
|
||||
% # # # # # ## # # # # # %
|
||||
% # # # # ##### # # # # #### # ### %
|
||||
% # # ##### # # # # # # # # # # %
|
||||
% # # # # # ## # # # # # # %
|
||||
% #### # ###### # # ##### ##### # # %
|
||||
% %
|
||||
% This file is part of openGJK. %
|
||||
% %
|
||||
% openGJK is free software: you can redistribute it and/or modify %
|
||||
% it under the terms of the GNU General Public License as published by %
|
||||
% the Free Software Foundation, either version 3 of the License, or %
|
||||
% any later version. %
|
||||
% %
|
||||
% openGJK is distributed in the hope that it will be useful, %
|
||||
% but WITHOUT ANY WARRANTY; without even the implied warranty of %
|
||||
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The %
|
||||
% GNU General Public License for more details. %
|
||||
% %
|
||||
% You should have received a copy of the GNU General Public License %
|
||||
% along with Foobar. If not, see <https://www.gnu.org/licenses/>. %
|
||||
% %
|
||||
% openGJK: open-source Gilbert-Johnson-Keerthi algorithm %
|
||||
% Copyright (C) Mattia Montanari 2018 - 2019 %
|
||||
% http://iel.eng.ox.ac.uk/?page_id=504 %
|
||||
% %
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% %
|
||||
% This file runs an example to illustrate how to cll the openGJK library %
|
||||
% withing Matlab. It assumes that a mex file openGJK is availalbe, see %
|
||||
% the runme.m script for information on how to compile it. %
|
||||
% The example computes the minimum distance between two polytopes in 3D, %
|
||||
% A and B, both defined as a list of points. %
|
||||
% %
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
|
||||
% DEFINE BODY A AS 3xN MATRIX, WHERE N IS THE NUMBER OF VERTICES OF BODY A
|
||||
A = [ 0.0 2.3 8.1 4.3 2.5 7.1 1.0 3.3 6.0
|
||||
5.5 1.0 4.0 5.0 1.0 1.0 1.5 0.5 1.4
|
||||
0.0 -2.0 2.4 2.2 2.3 2.4 0.3 0.3 0.2];
|
||||
|
||||
% DEFINE BODY B IN THE OPPOSITE QUADRANT OF BODY A
|
||||
B = -A;
|
||||
|
||||
% COMPUTE MINIMUM DISTANCE AND RETURN VALUE
|
||||
dist = openGJK( A, B );
|
||||
fprintf('The minimum distance between A and B is %.2f\n',dist);
|
||||
|
||||
% VISUALISE RESULTS
|
||||
% .. create new figure
|
||||
figure('units','centimeters', 'WindowStyle','normal', 'color','w',...
|
||||
'Position',[0 8.5 9 6],'defaultAxesColorOrder',parula,...
|
||||
'Renderer','opengl')
|
||||
% .. adjust properties
|
||||
axis equal tight off; hold all;
|
||||
% .. display body A
|
||||
DT = delaunayTriangulation(A');
|
||||
[K,~] = convexHull(DT);
|
||||
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
|
||||
'EdgeColor','none','FaceColor',[.4 1 .9 ],...
|
||||
'FaceLighting','flat' )
|
||||
% .. display body B
|
||||
DT = delaunayTriangulation(B');
|
||||
[K,~] = convexHull(DT);
|
||||
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
|
||||
'EdgeColor','none','FaceColor',[.4 1 .8 ],...
|
||||
'FaceLighting','flat' )
|
||||
% .. represent the computed distance as a sphere
|
||||
[x,y,z] = sphere(100);
|
||||
surf(x.*dist/2,y.*dist/2,z.*dist/2,'facecolor',[.9 .9 .9],...
|
||||
'EdgeColor','none','FaceLighting','flat','SpecularColorReflectance',0,...
|
||||
'SpecularStrength',1,'SpecularExponent',10,'facealpha',.7)
|
||||
% ... adjust point of view
|
||||
view(42,21)
|
||||
% ... add light
|
||||
light('Position',[5 -10 20],'Style','local');
|
||||
|
|
@ -1,79 +1,79 @@
|
|||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% ##### # # # %
|
||||
% #### ##### ###### # # # # # # # %
|
||||
% # # # # # ## # # # # # %
|
||||
% # # # # ##### # # # # #### # ### %
|
||||
% # # ##### # # # # # # # # # # %
|
||||
% # # # # # ## # # # # # # %
|
||||
% #### # ###### # # ##### ##### # # %
|
||||
% %
|
||||
% This file is part of openGJK. %
|
||||
% %
|
||||
% openGJK is free software: you can redistribute it and/or modify %
|
||||
% it under the terms of the GNU General Public License as published by %
|
||||
% the Free Software Foundation, either version 3 of the License, or %
|
||||
% any later version. %
|
||||
% %
|
||||
% openGJK is distributed in the hope that it will be useful, %
|
||||
% but WITHOUT ANY WARRANTY; without even the implied warranty of %
|
||||
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The %
|
||||
% GNU General Public License for more details. %
|
||||
% %
|
||||
% You should have received a copy of the GNU General Public License %
|
||||
% along with Foobar. If not, see <https://www.gnu.org/licenses/>. %
|
||||
% %
|
||||
% openGJK: open-source Gilbert-Johnson-Keerthi algorithm %
|
||||
% Copyright (C) Mattia Montanari 2018 - 2019 %
|
||||
% http://iel.eng.ox.ac.uk/?page_id=504 %
|
||||
% %
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% %
|
||||
% This file compiles a mex function from the openGJK library and runs an %
|
||||
% example. If the mex function cannot be compiled an error is returned. %
|
||||
% %
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
|
||||
% CLEAR ALL VARIABLES
|
||||
clearvars
|
||||
|
||||
% SELECT OPTIMISATION FLAG - FASTER BUT NOT SUITABLE FOR DEBUGGING
|
||||
if 0
|
||||
optflug = '-g'; %#ok<*UNRCH>
|
||||
else
|
||||
optflug = '-O';
|
||||
end
|
||||
% SELECT SILET COMPILATION MODE.
|
||||
if 1
|
||||
silflag = '-silent';
|
||||
else
|
||||
silflag = '-v';
|
||||
end
|
||||
|
||||
% TRY COMPILING MEX FILE
|
||||
fprintf('Compiling mex function... ')
|
||||
try
|
||||
mex(fullfile('..','lib','src','openGJK.c'),... % Source of openGJK
|
||||
'-largeArrayDims', ... % Support large arrays
|
||||
optflug, ... % Compiler flag for debug/optimisation
|
||||
fullfile('-I..','lib','include'),... % Folder to header files
|
||||
'-outdir', pwd,... % Ouput directory for writing mex function
|
||||
'-output', 'openGJK',... % Name of ouput mex file
|
||||
'-DMATLABDOESMEXSTUFF',... % Define variable for mex function in source files
|
||||
silflag ) % Silent/verbose flag
|
||||
|
||||
% File compiled without errors. Return path and name of mex file
|
||||
fprintf('completed!\n')
|
||||
fprintf('The following mex file has been generated:')
|
||||
fprintf('\t%s\n',[pwd,filesep,'openGJK.',mexext])
|
||||
catch
|
||||
% Build failed, refer to documentation
|
||||
fprintf('\n\n ERROR DETECTED! Mex file cannot be compiled.\n')
|
||||
fprintf('\tFor more information, see ')
|
||||
fprintf('<a href="http://www.mathworks.com/help/matlab/ref/mex.html">this documentation page</a>.\n\n')
|
||||
return
|
||||
end
|
||||
|
||||
% RUN EXAMPLE
|
||||
fprintf('Running example... ')
|
||||
main
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% ##### # # # %
|
||||
% #### ##### ###### # # # # # # # %
|
||||
% # # # # # ## # # # # # %
|
||||
% # # # # ##### # # # # #### # ### %
|
||||
% # # ##### # # # # # # # # # # %
|
||||
% # # # # # ## # # # # # # %
|
||||
% #### # ###### # # ##### ##### # # %
|
||||
% %
|
||||
% This file is part of openGJK. %
|
||||
% %
|
||||
% openGJK is free software: you can redistribute it and/or modify %
|
||||
% it under the terms of the GNU General Public License as published by %
|
||||
% the Free Software Foundation, either version 3 of the License, or %
|
||||
% any later version. %
|
||||
% %
|
||||
% openGJK is distributed in the hope that it will be useful, %
|
||||
% but WITHOUT ANY WARRANTY; without even the implied warranty of %
|
||||
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The %
|
||||
% GNU General Public License for more details. %
|
||||
% %
|
||||
% You should have received a copy of the GNU General Public License %
|
||||
% along with Foobar. If not, see <https://www.gnu.org/licenses/>. %
|
||||
% %
|
||||
% openGJK: open-source Gilbert-Johnson-Keerthi algorithm %
|
||||
% Copyright (C) Mattia Montanari 2018 - 2019 %
|
||||
% http://iel.eng.ox.ac.uk/?page_id=504 %
|
||||
% %
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% %
|
||||
% This file compiles a mex function from the openGJK library and runs an %
|
||||
% example. If the mex function cannot be compiled an error is returned. %
|
||||
% %
|
||||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
|
||||
% CLEAR ALL VARIABLES
|
||||
clearvars
|
||||
|
||||
% SELECT OPTIMISATION FLAG - FASTER BUT NOT SUITABLE FOR DEBUGGING
|
||||
if 0
|
||||
optflug = '-g'; %#ok<*UNRCH>
|
||||
else
|
||||
optflug = '-O';
|
||||
end
|
||||
% SELECT SILET COMPILATION MODE.
|
||||
if 1
|
||||
silflag = '-silent';
|
||||
else
|
||||
silflag = '-v';
|
||||
end
|
||||
|
||||
% TRY COMPILING MEX FILE
|
||||
fprintf('Compiling mex function... ')
|
||||
try
|
||||
mex(fullfile('..','..','lib','src','openGJK.c'),... % Source of openGJK
|
||||
'-largeArrayDims', ... % Support large arrays
|
||||
optflug, ... % Compiler flag for debug/optimisation
|
||||
fullfile('-I..','..','lib','include'),... % Folder to header files
|
||||
'-outdir', pwd,... % Ouput directory for writing mex function
|
||||
'-output', 'openGJK',... % Name of ouput mex file
|
||||
'-DMATLABDOESMEXSTUFF',... % Define variable for mex function in source files
|
||||
silflag ) % Silent/verbose flag
|
||||
|
||||
% File compiled without errors. Return path and name of mex file
|
||||
fprintf('completed!\n')
|
||||
fprintf('The following mex file has been generated:')
|
||||
fprintf('\t%s\n',[pwd,filesep,'openGJK.',mexext])
|
||||
catch
|
||||
% Build failed, refer to documentation
|
||||
fprintf('\n\n ERROR DETECTED! Mex file cannot be compiled.\n')
|
||||
fprintf('\tFor more information, see ')
|
||||
fprintf('<a href="http://www.mathworks.com/help/matlab/ref/mex.html">this documentation page</a>.\n\n')
|
||||
return
|
||||
end
|
||||
|
||||
% RUN EXAMPLE
|
||||
fprintf('Running example... ')
|
||||
main
|
||||
fprintf('completed!\n')
|
||||
|
|
@ -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
|
||||
|
|
@ -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
4271
lib/ext/predicates.c
File diff suppressed because it is too large
Load Diff
|
|
@ -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();
|
||||
|
|
@ -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
1009
lib/src/openGJK.c
File diff suppressed because it is too large
Load Diff
|
|
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue