New sub-algorithm
parent
095b9f49c9
commit
214b2c47d6
|
@ -20,7 +20,7 @@
|
||||||
# GNU General Public License for more details. #
|
# GNU General Public License for more details. #
|
||||||
# #
|
# #
|
||||||
# You should have received a copy of the GNU General Public License #
|
# You should have received a copy of the GNU General Public License #
|
||||||
# along with Foobar. If not, see <https://www.gnu.org/licenses/>. #
|
# along with openGJK. If not, see <https://www.gnu.org/licenses/>. #
|
||||||
# #
|
# #
|
||||||
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
|
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
|
||||||
# Copyright (C) Mattia Montanari 2018 - 2019 #
|
# Copyright (C) Mattia Montanari 2018 - 2019 #
|
||||||
|
@ -28,67 +28,57 @@
|
||||||
# #
|
# #
|
||||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||||
|
|
||||||
cmake_minimum_required(VERSION 3.5)
|
message( "[${PROJECT_NAME}] CMake setting ..")
|
||||||
project(openGJK)
|
|
||||||
|
|
||||||
set(CMAKE_C_STANDARD 11)
|
message(STATUS "Version : " ${openGJK_VERSION} )
|
||||||
|
message(STATUS "Build type : " ${CMAKE_BUILD_TYPE} )
|
||||||
|
|
||||||
message("[${CMAKE_PROJECT_NAME}] Welcome, please change user options if needed.")
|
# Select source files
|
||||||
|
set( SOURCE_FILES src/openGJK.c )
|
||||||
|
set( SOURCE_HEADS include/openGJK/openGJK.h)
|
||||||
|
|
||||||
# APPLY DEFAULT SETTINGS
|
IF(USE_PREDICATES)
|
||||||
if(NOT CMAKE_BUILD_TYPE)
|
# for adpative floating-point artim.
|
||||||
message("[${CMAKE_PROJECT_NAME}] Use default CMAKE_BUILD_TYPE")
|
set( SOURCE_FILES ${SOURCE_FILES} ext/predicates.c)
|
||||||
set(CMAKE_BUILD_TYPE Release)
|
set( SOURCE_HEADS ${SOURCE_HEADS} ext/predicates.h)
|
||||||
endif()
|
# 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
|
# PLATFORM-SPECIFIC SETTING
|
||||||
if (UNIX)
|
if (UNIX)
|
||||||
find_library(M_LIB m)
|
find_library(M_LIB m)
|
||||||
set(CMAKE_C_FLAGS "-lm")
|
set(CMAKE_C_FLAGS "-lm")
|
||||||
set(CMAKE_CXX_FLAGS "-lm")
|
|
||||||
else ()
|
else ()
|
||||||
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
|
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
|
||||||
endif ()
|
endif ()
|
||||||
|
|
||||||
# COMPILER SETTING
|
find_package(OpenMP)
|
||||||
IF(CMAKE_BUILD_TYPE MATCHES Release)
|
if (OPENMP_FOUND)
|
||||||
set(CMAKE_BUILD_TYPE Release)
|
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
|
||||||
ELSEIF(CMAKE_BUILD_TYPE MATCHES Debug)
|
set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
|
||||||
set(CMAKE_BUILD_TYPE Debug)
|
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()
|
ENDIF()
|
||||||
|
|
||||||
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
|
|
||||||
# using GCC
|
|
||||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra -finline-functions")
|
|
||||||
set(CMAKE_CXX_FLAGS_DEBUG "-g -DDEBUG")
|
|
||||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3")
|
|
||||||
|
|
||||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -finline-functions")
|
|
||||||
set(CMAKE_C_FLAGS_DEBUG "-g -DDEBUG")
|
|
||||||
set(CMAKE_C_FLAGS_RELEASE "-O3")
|
|
||||||
|
|
||||||
add_compile_options(-static-libgcc -static-libstdc++ )
|
|
||||||
add_definitions(-DMT)
|
|
||||||
elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
|
|
||||||
# using Visual Studio C++
|
|
||||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4131 /wd4701 /wd4255 /wd4710 /wd4820 /wd4711 /wd5045")
|
|
||||||
set(CMAKE_CXX_FLAGS_DEBUG "-DDEBUG /D_DEBUG /MDd /Zi /Ob0 /Od /RTC1")
|
|
||||||
set(CMAKE_CXX_FLAGS_RELEASE "/Ox")
|
|
||||||
|
|
||||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /wd4131 /wd4701 /wd4255 /wd4710 /wd4820 /wd4711 /wd5045")
|
|
||||||
set(CMAKE_C_FLAGS_DEBUG "-DDEBUG /D_DEBUG /MDd /Zi /Ob0 /Od /RTC1")
|
|
||||||
set(CMAKE_C_FLAGS_RELEASE "/Ox")
|
|
||||||
|
|
||||||
set(CMAKE_SUPPRESS_REGENERATION true)
|
|
||||||
|
|
||||||
endif()
|
|
||||||
|
|
||||||
# DEBUG FLAGS
|
|
||||||
IF(CMAKE_BUILD_TYPE MATCHES Debug)
|
|
||||||
add_definitions(-DDEBUG)
|
|
||||||
ENDIF()
|
|
||||||
|
|
||||||
# INCLUDE LIBRARY AND EXAMPLE DIR
|
|
||||||
add_subdirectory(lib)
|
|
||||||
add_subdirectory(example1_c)
|
|
||||||
|
|
193
README.md
193
README.md
|
@ -2,154 +2,30 @@
|
||||||
openGJK {#mainpage}
|
openGJK {#mainpage}
|
||||||
=======
|
=======
|
||||||
|
|
||||||
The openGJK library uses the Gilbert-Johnson-Keerthi (GJK) algorithm to
|
OpenGJK implements a new version of the Gilbert-Johnson-Keerthi (GJK) algorithm to
|
||||||
compute the minimum distance between convex polytopes. The
|
compute the minimum distance between convex polytopes. OpenGJK is a C library which was tested on Unix and Windows using different compilers for multi-threaded applications.
|
||||||
implementation follows the description presented in
|
|
||||||
"[Improving the GJK Algorithm for Faster and More Reliable Distance
|
|
||||||
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
|
Detailed information about the algorithm see "[Improving the GJK Algorithm for Faster and More Reliable Distance
|
||||||
out of the box: you can import it in your program and use it to measure
|
Queries Between Convex Objects. ACM Trans. on Graph. 2017](https://dl.acm.org/citation.cfm?id=3083724)".
|
||||||
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
|
When should I use openGJK?
|
||||||
|
--------------------------
|
||||||
|
|
||||||
openGJK comes in two flavours: *accurate* and *fast* (default). You can
|
OpenGJK is designed with speed, accuracy and robustness in mind and is therefore suitable for engineering, robotics and computer graphics simulations.
|
||||||
change before compiling by editing the main 'lib\CMakeLists.txt' file
|
Basically, openGJK can be used in any application where the distance between **any convex polytope** is required.
|
||||||
(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
|
Compile and run
|
||||||
|
---------------
|
||||||
|
|
||||||
This section presents three examples on how to use openGJK with C, C# and Matlab.
|
To compile the OpenGJK library create a build dir,
|
||||||
All the examples have been tested both Linux and Windows; the former used `make` and `gcc`,
|
and in the build dir call 'cmake ..' followed by 'make'. More details can be found in the INSTALL file.
|
||||||
the latter using `Visual studio 2017` and its compiler. Only x64 systems have been tested.
|
|
||||||
|
|
||||||
### C
|
There are examples for C, C# and Matlab in the `examples` folder. The INSTALL file provides information on how to run the examples.
|
||||||
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
|
Repository content
|
||||||
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:
|
This repository contains the following files and folders:
|
||||||
|
|
||||||
```
|
```
|
||||||
|
@ -191,7 +67,11 @@ This repository contains the following files and folders:
|
||||||
openGJK.c
|
openGJK.c
|
||||||
```
|
```
|
||||||
|
|
||||||
## Where to go next?
|
More information
|
||||||
|
----------------
|
||||||
|
|
||||||
|
[OpenGJK](http://iel.eng.ox.ac.uk/?page_id=504) was developed at the Impact Engineering Laboratory, University of Oxford.
|
||||||
|
|
||||||
|
|
||||||
A clear presentation of the GJK algorithm can be found in the
|
A clear presentation of the GJK algorithm can be found in the
|
||||||
book by **Van der Bergen** *Collision Detection in Interactive 3D
|
book by **Van der Bergen** *Collision Detection in Interactive 3D
|
||||||
|
@ -200,31 +80,16 @@ A clear presentation of the GJK algorithm can be found in the
|
||||||
More details about the GJK algorithm can be found in the original paper
|
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).
|
from Gilbert, Johnson and Keerthi [A fast procedure for computing the distance between complex objects in three-dimensional space](http://ieeexplore.ieee.org/document/2083/?arnumber=2083).
|
||||||
|
|
||||||
OpenGJK implements the GJK algorithm as described in: [Improving the GJK Algorithm for Faster and More Reliable Distance
|
|
||||||
Queries Between Convex Objects. ACM Trans. on Graph. 2017 ](https://dl.acm.org/citation.cfm?id=3083724). Refer to this papar for further details on the method.
|
|
||||||
|
|
||||||
|
How to cite openGJK
|
||||||
|
-------------------
|
||||||
|
|
||||||
|
If you use openGJK for your research please cite [OpenGJK for C, C# and Matlab: Reliable solutions to distance queries between convex bodies in three-dimensional space. SoftwareX. 2018](https://www.sciencedirect.com/science/article/pii/S2352711018300591).
|
||||||
|
|
||||||
## Licence
|
License
|
||||||
|
-------
|
||||||
|
|
||||||
This open-source edition of openGJK is released under the terms of
|
This project is licensed undert the GNU General Public License v3.0.
|
||||||
[CC BY-NC-SA 4.0](https://creativecommons.org/licenses/by-nc-sa/4.0/) License.
|
openGJK: open-source Gilbert-Johnson-Keerthi algorithm
|
||||||
This means that any software created with this library you must comply
|
Copyright (C) Mattia Montanari 2018 - 2019
|
||||||
with the terms of this licence. If you are seeking another licence please
|
http://iel.eng.ox.ac.uk/?page_id=504
|
||||||
contact the author at the address at the end of this file.
|
|
||||||
|
|
||||||
openGJK may use the geometric predicates from *Routines for Arbitrary
|
|
||||||
Precision Floating-point Arithmetic*, by Jonathan Richard Shewchuk,
|
|
||||||
whose source code is included in the file predicates.c of this
|
|
||||||
repository for convenience.
|
|
||||||
|
|
||||||
------------------------------------------------------------------------
|
|
||||||
openGJK, Copyright (c) 2018
|
|
||||||
|
|
||||||
Impact Engineering Laboratory
|
|
||||||
Department of Engineering Science
|
|
||||||
University of Oxford
|
|
||||||
Parks Road, Oxford, OX1 3PJ
|
|
||||||
|
|
||||||
mattia.montanari@eng.ox.ac.uk
|
|
||||||
------------------------------------------------------------------------
|
|
2491
doc/Doxyfile
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 |
|
@ -20,7 +20,7 @@
|
||||||
# GNU General Public License for more details. #
|
# GNU General Public License for more details. #
|
||||||
# #
|
# #
|
||||||
# You should have received a copy of the GNU General Public License #
|
# You should have received a copy of the GNU General Public License #
|
||||||
# along with Foobar. If not, see <https://www.gnu.org/licenses/>. #
|
# along with openGJK. If not, see <https://www.gnu.org/licenses/>. #
|
||||||
# #
|
# #
|
||||||
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
|
# openGJK: open-source Gilbert-Johnson-Keerthi algorithm #
|
||||||
# Copyright (C) Mattia Montanari 2018 - 2019 #
|
# Copyright (C) Mattia Montanari 2018 - 2019 #
|
||||||
|
@ -28,9 +28,13 @@
|
||||||
# #
|
# #
|
||||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||||
|
|
||||||
project(openGJKdemo)
|
project(openGJKdemo VERSION 1.0.0 LANGUAGES C)
|
||||||
|
|
||||||
message( "[${CMAKE_PROJECT_NAME}] Compiling the executable ..")
|
set(APPLICATION_NAME ${PROJECT_NAME})
|
||||||
|
set(CMAKE_C_STANDARD 11)
|
||||||
|
set(TEST_NAME ${PROJECT_NAME}_CTEST)
|
||||||
|
|
||||||
|
message( "[${PROJECT_NAME}] CMake setting ..")
|
||||||
|
|
||||||
# Set source file
|
# Set source file
|
||||||
set(SOURCE_FILES main.c )
|
set(SOURCE_FILES main.c )
|
||||||
|
@ -38,8 +42,37 @@ set(SOURCE_FILES main.c )
|
||||||
# Create the executable
|
# Create the executable
|
||||||
add_executable(demo ${SOURCE_FILES})
|
add_executable(demo ${SOURCE_FILES})
|
||||||
|
|
||||||
# Link to openGJK
|
# Copy input files after build
|
||||||
target_link_libraries(demo openGJKlib )
|
add_custom_command(
|
||||||
|
TARGET demo POST_BUILD
|
||||||
|
COMMAND ${CMAKE_COMMAND} -E copy
|
||||||
|
${CMAKE_SOURCE_DIR}/examples/c/userP.dat
|
||||||
|
${CMAKE_CURRENT_BINARY_DIR}/userP.dat
|
||||||
|
COMMAND ${CMAKE_COMMAND} -E copy
|
||||||
|
${CMAKE_SOURCE_DIR}/examples/c/userQ.dat
|
||||||
|
${CMAKE_CURRENT_BINARY_DIR}/userQ.dat)
|
||||||
|
|
||||||
# Report
|
# PLATFORM-SPECIFIC SETTING
|
||||||
message( ".. executable DONE!")
|
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}" )
|
|
@ -36,18 +36,19 @@
|
||||||
* *
|
* *
|
||||||
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
|
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file main.c
|
* @file main.c
|
||||||
* @author Mattia Montanari
|
* @author Mattia Montanari
|
||||||
* @date April 2018
|
* @date April 2018
|
||||||
* @brief File illustrating an application that invokes openGJK.
|
* @brief File illustrating an application that invokes openGJK.
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define _CRT_HAS_CXX17 0
|
#define _CRT_HAS_CXX17 0
|
||||||
|
#include <stdlib.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
/* For importing openGJK this is Step 1: include header in subfolder. */
|
/* For importing openGJK this is Step 1: include header in subfolder. */
|
||||||
#include "openGJK/openGJK.h"
|
#include "openGJK/openGJK.h"
|
||||||
|
|
||||||
#ifndef WIN32
|
#ifndef WIN32
|
||||||
|
@ -58,7 +59,7 @@
|
||||||
* @brief Function for reading input file with body's coordinates.
|
* @brief Function for reading input file with body's coordinates.
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
int readinput ( const char *inputfile, double ***pts, int * out ) {
|
int readinput(const char *inputfile, double ***pts, int * out) {
|
||||||
int npoints = 0;
|
int npoints = 0;
|
||||||
int idx = 0;
|
int idx = 0;
|
||||||
FILE *fp;
|
FILE *fp;
|
||||||
|
@ -81,13 +82,13 @@ int readinput ( const char *inputfile, double ***pts, int * out ) {
|
||||||
|
|
||||||
/* Allocate memory. */
|
/* Allocate memory. */
|
||||||
double **arr = (double **)malloc(npoints * sizeof(double *));
|
double **arr = (double **)malloc(npoints * sizeof(double *));
|
||||||
for (int i=0; i<npoints; i++)
|
for (int i = 0; i < npoints; i++)
|
||||||
arr[i] = (double *)malloc(3 * sizeof(double));
|
arr[i] = (double *)malloc(3 * sizeof(double));
|
||||||
|
|
||||||
/* Read and store vertices' coordinates. */
|
/* Read and store vertices' coordinates. */
|
||||||
for (idx = 0; idx < npoints; idx++)
|
for (idx = 0; idx < npoints; idx++)
|
||||||
{
|
{
|
||||||
if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3 )
|
if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3)
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -113,28 +114,28 @@ int main() {
|
||||||
struct simplex s;
|
struct simplex s;
|
||||||
/* Number of vertices defining body 1 and body 2, respectively. */
|
/* Number of vertices defining body 1 and body 2, respectively. */
|
||||||
int nvrtx1,
|
int nvrtx1,
|
||||||
nvrtx2;
|
nvrtx2;
|
||||||
/* Structures of body 1 and body 2, respectively. */
|
/* Structures of body 1 and body 2, respectively. */
|
||||||
struct bd bd1;
|
struct bd bd1;
|
||||||
struct bd bd2;
|
struct bd bd2;
|
||||||
/* Specify name of input files for body 1 and body 2, respectively. */
|
/* Specify name of input files for body 1 and body 2, respectively. */
|
||||||
char inputfileA[40] = "userP.dat",
|
char inputfileA[40] = "userP.dat",
|
||||||
inputfileB[40] = "userQ.dat";
|
inputfileB[40] = "userQ.dat";
|
||||||
/* Pointers to vertices' coordinates of body 1 and body 2, respectively. */
|
/* Pointers to vertices' coordinates of body 1 and body 2, respectively. */
|
||||||
double (**vrtx1) = NULL,
|
double(**vrtx1) = NULL,
|
||||||
(**vrtx2) = NULL;
|
(**vrtx2) = NULL;
|
||||||
|
|
||||||
/* For importing openGJK this is Step 2: adapt the data structure for the
|
/* For importing openGJK this is Step 2: adapt the data structure for the
|
||||||
* two bodies that will be passed to the GJK procedure. */
|
* two bodies that will be passed to the GJK procedure. */
|
||||||
|
|
||||||
/* Import coordinates of object 1. */
|
/* Import coordinates of object 1. */
|
||||||
if (readinput ( inputfileA, &vrtx1, &nvrtx1 ))
|
if (readinput(inputfileA, &vrtx1, &nvrtx1))
|
||||||
return (1);
|
return (1);
|
||||||
bd1.coord = vrtx1;
|
bd1.coord = vrtx1;
|
||||||
bd1.numpoints = nvrtx1;
|
bd1.numpoints = nvrtx1;
|
||||||
|
|
||||||
/* Import coordinates of object 2. */
|
/* Import coordinates of object 2. */
|
||||||
if (readinput ( inputfileB, &vrtx2, &nvrtx2 ))
|
if (readinput(inputfileB, &vrtx2, &nvrtx2))
|
||||||
return (1);
|
return (1);
|
||||||
bd2.coord = vrtx2;
|
bd2.coord = vrtx2;
|
||||||
bd2.numpoints = nvrtx2;
|
bd2.numpoints = nvrtx2;
|
||||||
|
@ -145,31 +146,31 @@ int main() {
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
/* Verify input of body A. */
|
/* Verify input of body A. */
|
||||||
for (int i = 0; i < bd1.numpoints; ++i) {
|
for (int i = 0; i < bd1.numpoints; ++i) {
|
||||||
printf ( "%.2f ", vrtx1[i][0]);
|
printf("%.2f ", vrtx1[i][0]);
|
||||||
printf ( "%.2f ", vrtx1[i][1]);
|
printf("%.2f ", vrtx1[i][1]);
|
||||||
printf ( "%.2f\n", bd1.coord[i][2]);
|
printf("%.2f\n", bd1.coord[i][2]);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Verify input of body B. */
|
/* Verify input of body B. */
|
||||||
for (int i = 0; i < bd2.numpoints; ++i) {
|
for (int i = 0; i < bd2.numpoints; ++i) {
|
||||||
printf ( "%.2f ", bd2.coord[i][0]);
|
printf("%.2f ", bd2.coord[i][0]);
|
||||||
printf ( "%.2f ", bd2.coord[i][1]);
|
printf("%.2f ", bd2.coord[i][1]);
|
||||||
printf ( "%.2f\n", bd2.coord[i][2]);
|
printf("%.2f\n", bd2.coord[i][2]);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* For importing openGJK this is Step 3: invoke the GJK procedure. */
|
/* For importing openGJK this is Step 3: invoke the GJK procedure. */
|
||||||
/* Compute squared distance using GJK algorithm. */
|
/* Compute squared distance using GJK algorithm. */
|
||||||
dd = gjk (bd1, bd2, &s);
|
dd = gjk(bd1, bd2, &s);
|
||||||
|
|
||||||
/* Print distance between objects. */
|
/* Print distance between objects. */
|
||||||
printf ("Distance between bodies %f\n", dd);
|
printf("Distance between bodies %f\n", dd);
|
||||||
|
|
||||||
/* Free memory */
|
/* Free memory */
|
||||||
for (int i=0; i<bd1.numpoints; i++)
|
for (int i = 0; i < bd1.numpoints; i++)
|
||||||
free(bd1.coord[i]);
|
free(bd1.coord[i]);
|
||||||
free(bd1.coord);
|
free(bd1.coord);
|
||||||
for (int i=0; i<bd2.numpoints; i++)
|
for (int i = 0; i < bd2.numpoints; i++)
|
||||||
free(bd2.coord[i]);
|
free(bd2.coord[i]);
|
||||||
free(bd2.coord);
|
free(bd2.coord);
|
||||||
|
|
|
@ -52,10 +52,10 @@ end
|
||||||
% TRY COMPILING MEX FILE
|
% TRY COMPILING MEX FILE
|
||||||
fprintf('Compiling mex function... ')
|
fprintf('Compiling mex function... ')
|
||||||
try
|
try
|
||||||
mex(fullfile('..','lib','src','openGJK.c'),... % Source of openGJK
|
mex(fullfile('..','..','lib','src','openGJK.c'),... % Source of openGJK
|
||||||
'-largeArrayDims', ... % Support large arrays
|
'-largeArrayDims', ... % Support large arrays
|
||||||
optflug, ... % Compiler flag for debug/optimisation
|
optflug, ... % Compiler flag for debug/optimisation
|
||||||
fullfile('-I..','lib','include'),... % Folder to header files
|
fullfile('-I..','..','lib','include'),... % Folder to header files
|
||||||
'-outdir', pwd,... % Ouput directory for writing mex function
|
'-outdir', pwd,... % Ouput directory for writing mex function
|
||||||
'-output', 'openGJK',... % Name of ouput mex file
|
'-output', 'openGJK',... % Name of ouput mex file
|
||||||
'-DMATLABDOESMEXSTUFF',... % Define variable for mex function in source files
|
'-DMATLABDOESMEXSTUFF',... % Define variable for mex function in source files
|
|
@ -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