Merge pull request #21 from MattiaMontanari/ce
Minimalistic community edition (CE) libraryfixes-turtlebasket
Mattia Montanari
GitHub
commit
1b848558f8
174
CMakeLists.txt
174
CMakeLists.txt
|
|
@ -1,92 +1,82 @@
|
|||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
# ##### # # # #
|
||||
# #### ##### ###### # # # # # # # #
|
||||
# # # # # # ## # # # # # #
|
||||
# # # # # ##### # # # # #### # ### #
|
||||
# # # ##### # # # # # # # # # # #
|
||||
# # # # # # ## # # # # # # #
|
||||
# #### # ###### # # ##### ##### # # #
|
||||
# #
|
||||
# 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 - 2020 #
|
||||
# http://iel.eng.ox.ac.uk/?page_id=504 #
|
||||
# #
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
|
||||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
set(CMAKE_POLICY_DEFAULT_CMP0079 NEW)
|
||||
set(LIBRARY_VERSION "2.0.3")
|
||||
|
||||
project(openGJKlib VERSION ${LIBRARY_VERSION} LANGUAGES C)
|
||||
|
||||
set(CMAKE_C_STANDARD 11)
|
||||
|
||||
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
|
||||
include(CMakeDefaults)
|
||||
include(CompilerFlags)
|
||||
include(PlatformDefaults)
|
||||
|
||||
message( "[${PROJECT_NAME}] CMake setting ..")
|
||||
message(STATUS "Version : " ${CMAKE_PROJECT_VERSION} )
|
||||
message(STATUS "Build type : " ${CMAKE_BUILD_TYPE} )
|
||||
|
||||
# Specify project specific and user custum options
|
||||
include(CMakeProjectOptions)
|
||||
|
||||
set( SOURCE_FILES src/openGJK.c )
|
||||
set( SOURCE_HEADS include/openGJK/openGJK.h)
|
||||
|
||||
IF(BUILD_STATIC_LIB)
|
||||
add_library(${PROJECT_NAME} STATIC ${SOURCE_FILES} ${SOURCE_HEADS})
|
||||
add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=FALSE)
|
||||
ELSE()
|
||||
add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${SOURCE_HEADS})
|
||||
add_definitions(-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=TRUE -DBUILD_SHARED_LIBS=TRUE)
|
||||
ENDIF(BUILD_STATIC_LIB)
|
||||
|
||||
set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER ${SOURCE_HEADS})
|
||||
|
||||
# Add compiler flags
|
||||
include(CompilerFlags)
|
||||
|
||||
# Install setup
|
||||
install(TARGETS ${PROJECT_NAME} PERMISSIONS WORLD_WRITE )
|
||||
|
||||
find_package(OpenMP REQUIRED)
|
||||
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()
|
||||
|
||||
# Link include file
|
||||
target_include_directories( ${PROJECT_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
|
||||
|
||||
target_link_libraries(${PROJECT_NAME} ${CMOCKA_LIBRARY} )
|
||||
|
||||
set(DESTDIR "/usr")
|
||||
INSTALL(TARGETS ${PROJECT_NAME}
|
||||
LIBRARY DESTINATION "${DESTDIR}/lib"
|
||||
PUBLIC_HEADER DESTINATION "${DESTDIR}/include"
|
||||
)
|
||||
|
||||
if (WITH_EXAMPLES)
|
||||
add_subdirectory(examples/c)
|
||||
endif (WITH_EXAMPLES)
|
||||
|
||||
message(STATUS "Completed CMake setting for ${PROJECT_NAME}" )
|
||||
# _____ _ _ __ #
|
||||
# / ____| | | |/ / #
|
||||
# ___ _ __ ___ _ __ | | __ | | ' / #
|
||||
# / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < #
|
||||
# | (_) | |_) | __/ | | | |__| | |__| | . \ #
|
||||
# \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ #
|
||||
# | | #
|
||||
# |_| #
|
||||
# #
|
||||
# Copyright 2022 Mattia Montanari, University of Oxford #
|
||||
# #
|
||||
# This program 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. You should have received a copy #
|
||||
# of the GNU General Public License along with this program. If not, visit #
|
||||
# #
|
||||
# https://www.gnu.org/licenses/ #
|
||||
# #
|
||||
# This program 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 GNU General Public License for details. #
|
||||
|
||||
cmake_minimum_required(VERSION 2.8)
|
||||
cmake_policy(SET CMP0048 NEW)
|
||||
cmake_policy(SET CMP0079 NEW)
|
||||
|
||||
option(BUILD_EXAMPLE "Build demo" ON)
|
||||
|
||||
project(lib_opengjk_ce
|
||||
LANGUAGES C
|
||||
VERSION ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}
|
||||
DESCRIPTION "openGJK library"
|
||||
HOMEPAGE_URL "https://mattiamontanari.com/opengjk"
|
||||
)
|
||||
|
||||
set( C_STANDARD 99)
|
||||
set( CMAKE_CXX_VISIBILITY_PRESET hidden)
|
||||
set( CMAKE_VISIBILITY_INLINES_HIDDEN 1)
|
||||
set( CMAKE_POLICY_DEFAULT_CMP0079 NEW)
|
||||
set( CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
|
||||
set( CMAKE_INCLUDE_CURRENT_DIR TRUE)
|
||||
|
||||
set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -pedantic -Wunused-macros")
|
||||
set( CMAKE_C_FLAGS_DEBUG "-O0 -g -Wall -Wno-unused-command-line-argument")
|
||||
set( CMAKE_C_FLAGS_RELEASE "-O3 -Werror -Wno-unused-command-line-argument")
|
||||
|
||||
set( GK_PUBLIC_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
STATIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/openGJK.c
|
||||
${GK_PUBLIC_HEADER_DIR}/openGJK/openGJK.h
|
||||
)
|
||||
|
||||
target_include_directories(
|
||||
${PROJECT_NAME}
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
${CMAKE_CURRENT_BINARY_DIR}
|
||||
)
|
||||
|
||||
set_target_properties(${PROJECT_NAME}
|
||||
PROPERTIES
|
||||
PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/include/openGJK/openGJK.h
|
||||
)
|
||||
|
||||
if(BUILD_EXAMPLE)
|
||||
add_subdirectory(examples/c)
|
||||
endif(BUILD_EXAMPLE)
|
||||
|
||||
if (UNIX)
|
||||
install(TARGETS ${PROJECT_NAME} PERMISSIONS WORLD_WRITE )
|
||||
set(DESTDIR "/usr")
|
||||
INSTALL(TARGETS ${PROJECT_NAME}
|
||||
LIBRARY DESTINATION "${DESTDIR}/lib"
|
||||
PUBLIC_HEADER DESTINATION "${DESTDIR}/include"
|
||||
)
|
||||
endif (UNIX)
|
||||
|
||||
# Wrap up feedback on setup
|
||||
message(STATUS "Version : " ${CMAKE_PROJECT_VERSION} )
|
||||
message(STATUS "Build type : " ${CMAKE_BUILD_TYPE} )
|
||||
674
COPYING
674
COPYING
|
|
@ -1,674 +0,0 @@
|
|||
GNU GENERAL PUBLIC LICENSE
|
||||
Version 3, 29 June 2007
|
||||
|
||||
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
|
||||
Everyone is permitted to copy and distribute verbatim copies
|
||||
of this license document, but changing it is not allowed.
|
||||
|
||||
Preamble
|
||||
|
||||
The GNU General Public License is a free, copyleft license for
|
||||
software and other kinds of works.
|
||||
|
||||
The licenses for most software and other practical works are designed
|
||||
to take away your freedom to share and change the works. By contrast,
|
||||
the GNU General Public License is intended to guarantee your freedom to
|
||||
share and change all versions of a program--to make sure it remains free
|
||||
software for all its users. We, the Free Software Foundation, use the
|
||||
GNU General Public License for most of our software; it applies also to
|
||||
any other work released this way by its authors. You can apply it to
|
||||
your programs, too.
|
||||
|
||||
When we speak of free software, we are referring to freedom, not
|
||||
price. Our General Public Licenses are designed to make sure that you
|
||||
have the freedom to distribute copies of free software (and charge for
|
||||
them if you wish), that you receive source code or can get it if you
|
||||
want it, that you can change the software or use pieces of it in new
|
||||
free programs, and that you know you can do these things.
|
||||
|
||||
To protect your rights, we need to prevent others from denying you
|
||||
these rights or asking you to surrender the rights. Therefore, you have
|
||||
certain responsibilities if you distribute copies of the software, or if
|
||||
you modify it: responsibilities to respect the freedom of others.
|
||||
|
||||
For example, if you distribute copies of such a program, whether
|
||||
gratis or for a fee, you must pass on to the recipients the same
|
||||
freedoms that you received. You must make sure that they, too, receive
|
||||
or can get the source code. And you must show them these terms so they
|
||||
know their rights.
|
||||
|
||||
Developers that use the GNU GPL protect your rights with two steps:
|
||||
(1) assert copyright on the software, and (2) offer you this License
|
||||
giving you legal permission to copy, distribute and/or modify it.
|
||||
|
||||
For the developers' and authors' protection, the GPL clearly explains
|
||||
that there is no warranty for this free software. For both users' and
|
||||
authors' sake, the GPL requires that modified versions be marked as
|
||||
changed, so that their problems will not be attributed erroneously to
|
||||
authors of previous versions.
|
||||
|
||||
Some devices are designed to deny users access to install or run
|
||||
modified versions of the software inside them, although the manufacturer
|
||||
can do so. This is fundamentally incompatible with the aim of
|
||||
protecting users' freedom to change the software. The systematic
|
||||
pattern of such abuse occurs in the area of products for individuals to
|
||||
use, which is precisely where it is most unacceptable. Therefore, we
|
||||
have designed this version of the GPL to prohibit the practice for those
|
||||
products. If such problems arise substantially in other domains, we
|
||||
stand ready to extend this provision to those domains in future versions
|
||||
of the GPL, as needed to protect the freedom of users.
|
||||
|
||||
Finally, every program is threatened constantly by software patents.
|
||||
States should not allow patents to restrict development and use of
|
||||
software on general-purpose computers, but in those that do, we wish to
|
||||
avoid the special danger that patents applied to a free program could
|
||||
make it effectively proprietary. To prevent this, the GPL assures that
|
||||
patents cannot be used to render the program non-free.
|
||||
|
||||
The precise terms and conditions for copying, distribution and
|
||||
modification follow.
|
||||
|
||||
TERMS AND CONDITIONS
|
||||
|
||||
0. Definitions.
|
||||
|
||||
"This License" refers to version 3 of the GNU General Public License.
|
||||
|
||||
"Copyright" also means copyright-like laws that apply to other kinds of
|
||||
works, such as semiconductor masks.
|
||||
|
||||
"The Program" refers to any copyrightable work licensed under this
|
||||
License. Each licensee is addressed as "you". "Licensees" and
|
||||
"recipients" may be individuals or organizations.
|
||||
|
||||
To "modify" a work means to copy from or adapt all or part of the work
|
||||
in a fashion requiring copyright permission, other than the making of an
|
||||
exact copy. The resulting work is called a "modified version" of the
|
||||
earlier work or a work "based on" the earlier work.
|
||||
|
||||
A "covered work" means either the unmodified Program or a work based
|
||||
on the Program.
|
||||
|
||||
To "propagate" a work means to do anything with it that, without
|
||||
permission, would make you directly or secondarily liable for
|
||||
infringement under applicable copyright law, except executing it on a
|
||||
computer or modifying a private copy. Propagation includes copying,
|
||||
distribution (with or without modification), making available to the
|
||||
public, and in some countries other activities as well.
|
||||
|
||||
To "convey" a work means any kind of propagation that enables other
|
||||
parties to make or receive copies. Mere interaction with a user through
|
||||
a computer network, with no transfer of a copy, is not conveying.
|
||||
|
||||
An interactive user interface displays "Appropriate Legal Notices"
|
||||
to the extent that it includes a convenient and prominently visible
|
||||
feature that (1) displays an appropriate copyright notice, and (2)
|
||||
tells the user that there is no warranty for the work (except to the
|
||||
extent that warranties are provided), that licensees may convey the
|
||||
work under this License, and how to view a copy of this License. If
|
||||
the interface presents a list of user commands or options, such as a
|
||||
menu, a prominent item in the list meets this criterion.
|
||||
|
||||
1. Source Code.
|
||||
|
||||
The "source code" for a work means the preferred form of the work
|
||||
for making modifications to it. "Object code" means any non-source
|
||||
form of a work.
|
||||
|
||||
A "Standard Interface" means an interface that either is an official
|
||||
standard defined by a recognized standards body, or, in the case of
|
||||
interfaces specified for a particular programming language, one that
|
||||
is widely used among developers working in that language.
|
||||
|
||||
The "System Libraries" of an executable work include anything, other
|
||||
than the work as a whole, that (a) is included in the normal form of
|
||||
packaging a Major Component, but which is not part of that Major
|
||||
Component, and (b) serves only to enable use of the work with that
|
||||
Major Component, or to implement a Standard Interface for which an
|
||||
implementation is available to the public in source code form. A
|
||||
"Major Component", in this context, means a major essential component
|
||||
(kernel, window system, and so on) of the specific operating system
|
||||
(if any) on which the executable work runs, or a compiler used to
|
||||
produce the work, or an object code interpreter used to run it.
|
||||
|
||||
The "Corresponding Source" for a work in object code form means all
|
||||
the source code needed to generate, install, and (for an executable
|
||||
work) run the object code and to modify the work, including scripts to
|
||||
control those activities. However, it does not include the work's
|
||||
System Libraries, or general-purpose tools or generally available free
|
||||
programs which are used unmodified in performing those activities but
|
||||
which are not part of the work. For example, Corresponding Source
|
||||
includes interface definition files associated with source files for
|
||||
the work, and the source code for shared libraries and dynamically
|
||||
linked subprograms that the work is specifically designed to require,
|
||||
such as by intimate data communication or control flow between those
|
||||
subprograms and other parts of the work.
|
||||
|
||||
The Corresponding Source need not include anything that users
|
||||
can regenerate automatically from other parts of the Corresponding
|
||||
Source.
|
||||
|
||||
The Corresponding Source for a work in source code form is that
|
||||
same work.
|
||||
|
||||
2. Basic Permissions.
|
||||
|
||||
All rights granted under this License are granted for the term of
|
||||
copyright on the Program, and are irrevocable provided the stated
|
||||
conditions are met. This License explicitly affirms your unlimited
|
||||
permission to run the unmodified Program. The output from running a
|
||||
covered work is covered by this License only if the output, given its
|
||||
content, constitutes a covered work. This License acknowledges your
|
||||
rights of fair use or other equivalent, as provided by copyright law.
|
||||
|
||||
You may make, run and propagate covered works that you do not
|
||||
convey, without conditions so long as your license otherwise remains
|
||||
in force. You may convey covered works to others for the sole purpose
|
||||
of having them make modifications exclusively for you, or provide you
|
||||
with facilities for running those works, provided that you comply with
|
||||
the terms of this License in conveying all material for which you do
|
||||
not control copyright. Those thus making or running the covered works
|
||||
for you must do so exclusively on your behalf, under your direction
|
||||
and control, on terms that prohibit them from making any copies of
|
||||
your copyrighted material outside their relationship with you.
|
||||
|
||||
Conveying under any other circumstances is permitted solely under
|
||||
the conditions stated below. Sublicensing is not allowed; section 10
|
||||
makes it unnecessary.
|
||||
|
||||
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
|
||||
|
||||
No covered work shall be deemed part of an effective technological
|
||||
measure under any applicable law fulfilling obligations under article
|
||||
11 of the WIPO copyright treaty adopted on 20 December 1996, or
|
||||
similar laws prohibiting or restricting circumvention of such
|
||||
measures.
|
||||
|
||||
When you convey a covered work, you waive any legal power to forbid
|
||||
circumvention of technological measures to the extent such circumvention
|
||||
is effected by exercising rights under this License with respect to
|
||||
the covered work, and you disclaim any intention to limit operation or
|
||||
modification of the work as a means of enforcing, against the work's
|
||||
users, your or third parties' legal rights to forbid circumvention of
|
||||
technological measures.
|
||||
|
||||
4. Conveying Verbatim Copies.
|
||||
|
||||
You may convey verbatim copies of the Program's source code as you
|
||||
receive it, in any medium, provided that you conspicuously and
|
||||
appropriately publish on each copy an appropriate copyright notice;
|
||||
keep intact all notices stating that this License and any
|
||||
non-permissive terms added in accord with section 7 apply to the code;
|
||||
keep intact all notices of the absence of any warranty; and give all
|
||||
recipients a copy of this License along with the Program.
|
||||
|
||||
You may charge any price or no price for each copy that you convey,
|
||||
and you may offer support or warranty protection for a fee.
|
||||
|
||||
5. Conveying Modified Source Versions.
|
||||
|
||||
You may convey a work based on the Program, or the modifications to
|
||||
produce it from the Program, in the form of source code under the
|
||||
terms of section 4, provided that you also meet all of these conditions:
|
||||
|
||||
a) The work must carry prominent notices stating that you modified
|
||||
it, and giving a relevant date.
|
||||
|
||||
b) The work must carry prominent notices stating that it is
|
||||
released under this License and any conditions added under section
|
||||
7. This requirement modifies the requirement in section 4 to
|
||||
"keep intact all notices".
|
||||
|
||||
c) You must license the entire work, as a whole, under this
|
||||
License to anyone who comes into possession of a copy. This
|
||||
License will therefore apply, along with any applicable section 7
|
||||
additional terms, to the whole of the work, and all its parts,
|
||||
regardless of how they are packaged. This License gives no
|
||||
permission to license the work in any other way, but it does not
|
||||
invalidate such permission if you have separately received it.
|
||||
|
||||
d) If the work has interactive user interfaces, each must display
|
||||
Appropriate Legal Notices; however, if the Program has interactive
|
||||
interfaces that do not display Appropriate Legal Notices, your
|
||||
work need not make them do so.
|
||||
|
||||
A compilation of a covered work with other separate and independent
|
||||
works, which are not by their nature extensions of the covered work,
|
||||
and which are not combined with it such as to form a larger program,
|
||||
in or on a volume of a storage or distribution medium, is called an
|
||||
"aggregate" if the compilation and its resulting copyright are not
|
||||
used to limit the access or legal rights of the compilation's users
|
||||
beyond what the individual works permit. Inclusion of a covered work
|
||||
in an aggregate does not cause this License to apply to the other
|
||||
parts of the aggregate.
|
||||
|
||||
6. Conveying Non-Source Forms.
|
||||
|
||||
You may convey a covered work in object code form under the terms
|
||||
of sections 4 and 5, provided that you also convey the
|
||||
machine-readable Corresponding Source under the terms of this License,
|
||||
in one of these ways:
|
||||
|
||||
a) Convey the object code in, or embodied in, a physical product
|
||||
(including a physical distribution medium), accompanied by the
|
||||
Corresponding Source fixed on a durable physical medium
|
||||
customarily used for software interchange.
|
||||
|
||||
b) Convey the object code in, or embodied in, a physical product
|
||||
(including a physical distribution medium), accompanied by a
|
||||
written offer, valid for at least three years and valid for as
|
||||
long as you offer spare parts or customer support for that product
|
||||
model, to give anyone who possesses the object code either (1) a
|
||||
copy of the Corresponding Source for all the software in the
|
||||
product that is covered by this License, on a durable physical
|
||||
medium customarily used for software interchange, for a price no
|
||||
more than your reasonable cost of physically performing this
|
||||
conveying of source, or (2) access to copy the
|
||||
Corresponding Source from a network server at no charge.
|
||||
|
||||
c) Convey individual copies of the object code with a copy of the
|
||||
written offer to provide the Corresponding Source. This
|
||||
alternative is allowed only occasionally and noncommercially, and
|
||||
only if you received the object code with such an offer, in accord
|
||||
with subsection 6b.
|
||||
|
||||
d) Convey the object code by offering access from a designated
|
||||
place (gratis or for a charge), and offer equivalent access to the
|
||||
Corresponding Source in the same way through the same place at no
|
||||
further charge. You need not require recipients to copy the
|
||||
Corresponding Source along with the object code. If the place to
|
||||
copy the object code is a network server, the Corresponding Source
|
||||
may be on a different server (operated by you or a third party)
|
||||
that supports equivalent copying facilities, provided you maintain
|
||||
clear directions next to the object code saying where to find the
|
||||
Corresponding Source. Regardless of what server hosts the
|
||||
Corresponding Source, you remain obligated to ensure that it is
|
||||
available for as long as needed to satisfy these requirements.
|
||||
|
||||
e) Convey the object code using peer-to-peer transmission, provided
|
||||
you inform other peers where the object code and Corresponding
|
||||
Source of the work are being offered to the general public at no
|
||||
charge under subsection 6d.
|
||||
|
||||
A separable portion of the object code, whose source code is excluded
|
||||
from the Corresponding Source as a System Library, need not be
|
||||
included in conveying the object code work.
|
||||
|
||||
A "User Product" is either (1) a "consumer product", which means any
|
||||
tangible personal property which is normally used for personal, family,
|
||||
or household purposes, or (2) anything designed or sold for incorporation
|
||||
into a dwelling. In determining whether a product is a consumer product,
|
||||
doubtful cases shall be resolved in favor of coverage. For a particular
|
||||
product received by a particular user, "normally used" refers to a
|
||||
typical or common use of that class of product, regardless of the status
|
||||
of the particular user or of the way in which the particular user
|
||||
actually uses, or expects or is expected to use, the product. A product
|
||||
is a consumer product regardless of whether the product has substantial
|
||||
commercial, industrial or non-consumer uses, unless such uses represent
|
||||
the only significant mode of use of the product.
|
||||
|
||||
"Installation Information" for a User Product means any methods,
|
||||
procedures, authorization keys, or other information required to install
|
||||
and execute modified versions of a covered work in that User Product from
|
||||
a modified version of its Corresponding Source. The information must
|
||||
suffice to ensure that the continued functioning of the modified object
|
||||
code is in no case prevented or interfered with solely because
|
||||
modification has been made.
|
||||
|
||||
If you convey an object code work under this section in, or with, or
|
||||
specifically for use in, a User Product, and the conveying occurs as
|
||||
part of a transaction in which the right of possession and use of the
|
||||
User Product is transferred to the recipient in perpetuity or for a
|
||||
fixed term (regardless of how the transaction is characterized), the
|
||||
Corresponding Source conveyed under this section must be accompanied
|
||||
by the Installation Information. But this requirement does not apply
|
||||
if neither you nor any third party retains the ability to install
|
||||
modified object code on the User Product (for example, the work has
|
||||
been installed in ROM).
|
||||
|
||||
The requirement to provide Installation Information does not include a
|
||||
requirement to continue to provide support service, warranty, or updates
|
||||
for a work that has been modified or installed by the recipient, or for
|
||||
the User Product in which it has been modified or installed. Access to a
|
||||
network may be denied when the modification itself materially and
|
||||
adversely affects the operation of the network or violates the rules and
|
||||
protocols for communication across the network.
|
||||
|
||||
Corresponding Source conveyed, and Installation Information provided,
|
||||
in accord with this section must be in a format that is publicly
|
||||
documented (and with an implementation available to the public in
|
||||
source code form), and must require no special password or key for
|
||||
unpacking, reading or copying.
|
||||
|
||||
7. Additional Terms.
|
||||
|
||||
"Additional permissions" are terms that supplement the terms of this
|
||||
License by making exceptions from one or more of its conditions.
|
||||
Additional permissions that are applicable to the entire Program shall
|
||||
be treated as though they were included in this License, to the extent
|
||||
that they are valid under applicable law. If additional permissions
|
||||
apply only to part of the Program, that part may be used separately
|
||||
under those permissions, but the entire Program remains governed by
|
||||
this License without regard to the additional permissions.
|
||||
|
||||
When you convey a copy of a covered work, you may at your option
|
||||
remove any additional permissions from that copy, or from any part of
|
||||
it. (Additional permissions may be written to require their own
|
||||
removal in certain cases when you modify the work.) You may place
|
||||
additional permissions on material, added by you to a covered work,
|
||||
for which you have or can give appropriate copyright permission.
|
||||
|
||||
Notwithstanding any other provision of this License, for material you
|
||||
add to a covered work, you may (if authorized by the copyright holders of
|
||||
that material) supplement the terms of this License with terms:
|
||||
|
||||
a) Disclaiming warranty or limiting liability differently from the
|
||||
terms of sections 15 and 16 of this License; or
|
||||
|
||||
b) Requiring preservation of specified reasonable legal notices or
|
||||
author attributions in that material or in the Appropriate Legal
|
||||
Notices displayed by works containing it; or
|
||||
|
||||
c) Prohibiting misrepresentation of the origin of that material, or
|
||||
requiring that modified versions of such material be marked in
|
||||
reasonable ways as different from the original version; or
|
||||
|
||||
d) Limiting the use for publicity purposes of names of licensors or
|
||||
authors of the material; or
|
||||
|
||||
e) Declining to grant rights under trademark law for use of some
|
||||
trade names, trademarks, or service marks; or
|
||||
|
||||
f) Requiring indemnification of licensors and authors of that
|
||||
material by anyone who conveys the material (or modified versions of
|
||||
it) with contractual assumptions of liability to the recipient, for
|
||||
any liability that these contractual assumptions directly impose on
|
||||
those licensors and authors.
|
||||
|
||||
All other non-permissive additional terms are considered "further
|
||||
restrictions" within the meaning of section 10. If the Program as you
|
||||
received it, or any part of it, contains a notice stating that it is
|
||||
governed by this License along with a term that is a further
|
||||
restriction, you may remove that term. If a license document contains
|
||||
a further restriction but permits relicensing or conveying under this
|
||||
License, you may add to a covered work material governed by the terms
|
||||
of that license document, provided that the further restriction does
|
||||
not survive such relicensing or conveying.
|
||||
|
||||
If you add terms to a covered work in accord with this section, you
|
||||
must place, in the relevant source files, a statement of the
|
||||
additional terms that apply to those files, or a notice indicating
|
||||
where to find the applicable terms.
|
||||
|
||||
Additional terms, permissive or non-permissive, may be stated in the
|
||||
form of a separately written license, or stated as exceptions;
|
||||
the above requirements apply either way.
|
||||
|
||||
8. Termination.
|
||||
|
||||
You may not propagate or modify a covered work except as expressly
|
||||
provided under this License. Any attempt otherwise to propagate or
|
||||
modify it is void, and will automatically terminate your rights under
|
||||
this License (including any patent licenses granted under the third
|
||||
paragraph of section 11).
|
||||
|
||||
However, if you cease all violation of this License, then your
|
||||
license from a particular copyright holder is reinstated (a)
|
||||
provisionally, unless and until the copyright holder explicitly and
|
||||
finally terminates your license, and (b) permanently, if the copyright
|
||||
holder fails to notify you of the violation by some reasonable means
|
||||
prior to 60 days after the cessation.
|
||||
|
||||
Moreover, your license from a particular copyright holder is
|
||||
reinstated permanently if the copyright holder notifies you of the
|
||||
violation by some reasonable means, this is the first time you have
|
||||
received notice of violation of this License (for any work) from that
|
||||
copyright holder, and you cure the violation prior to 30 days after
|
||||
your receipt of the notice.
|
||||
|
||||
Termination of your rights under this section does not terminate the
|
||||
licenses of parties who have received copies or rights from you under
|
||||
this License. If your rights have been terminated and not permanently
|
||||
reinstated, you do not qualify to receive new licenses for the same
|
||||
material under section 10.
|
||||
|
||||
9. Acceptance Not Required for Having Copies.
|
||||
|
||||
You are not required to accept this License in order to receive or
|
||||
run a copy of the Program. Ancillary propagation of a covered work
|
||||
occurring solely as a consequence of using peer-to-peer transmission
|
||||
to receive a copy likewise does not require acceptance. However,
|
||||
nothing other than this License grants you permission to propagate or
|
||||
modify any covered work. These actions infringe copyright if you do
|
||||
not accept this License. Therefore, by modifying or propagating a
|
||||
covered work, you indicate your acceptance of this License to do so.
|
||||
|
||||
10. Automatic Licensing of Downstream Recipients.
|
||||
|
||||
Each time you convey a covered work, the recipient automatically
|
||||
receives a license from the original licensors, to run, modify and
|
||||
propagate that work, subject to this License. You are not responsible
|
||||
for enforcing compliance by third parties with this License.
|
||||
|
||||
An "entity transaction" is a transaction transferring control of an
|
||||
organization, or substantially all assets of one, or subdividing an
|
||||
organization, or merging organizations. If propagation of a covered
|
||||
work results from an entity transaction, each party to that
|
||||
transaction who receives a copy of the work also receives whatever
|
||||
licenses to the work the party's predecessor in interest had or could
|
||||
give under the previous paragraph, plus a right to possession of the
|
||||
Corresponding Source of the work from the predecessor in interest, if
|
||||
the predecessor has it or can get it with reasonable efforts.
|
||||
|
||||
You may not impose any further restrictions on the exercise of the
|
||||
rights granted or affirmed under this License. For example, you may
|
||||
not impose a license fee, royalty, or other charge for exercise of
|
||||
rights granted under this License, and you may not initiate litigation
|
||||
(including a cross-claim or counterclaim in a lawsuit) alleging that
|
||||
any patent claim is infringed by making, using, selling, offering for
|
||||
sale, or importing the Program or any portion of it.
|
||||
|
||||
11. Patents.
|
||||
|
||||
A "contributor" is a copyright holder who authorizes use under this
|
||||
License of the Program or a work on which the Program is based. The
|
||||
work thus licensed is called the contributor's "contributor version".
|
||||
|
||||
A contributor's "essential patent claims" are all patent claims
|
||||
owned or controlled by the contributor, whether already acquired or
|
||||
hereafter acquired, that would be infringed by some manner, permitted
|
||||
by this License, of making, using, or selling its contributor version,
|
||||
but do not include claims that would be infringed only as a
|
||||
consequence of further modification of the contributor version. For
|
||||
purposes of this definition, "control" includes the right to grant
|
||||
patent sublicenses in a manner consistent with the requirements of
|
||||
this License.
|
||||
|
||||
Each contributor grants you a non-exclusive, worldwide, royalty-free
|
||||
patent license under the contributor's essential patent claims, to
|
||||
make, use, sell, offer for sale, import and otherwise run, modify and
|
||||
propagate the contents of its contributor version.
|
||||
|
||||
In the following three paragraphs, a "patent license" is any express
|
||||
agreement or commitment, however denominated, not to enforce a patent
|
||||
(such as an express permission to practice a patent or covenant not to
|
||||
sue for patent infringement). To "grant" such a patent license to a
|
||||
party means to make such an agreement or commitment not to enforce a
|
||||
patent against the party.
|
||||
|
||||
If you convey a covered work, knowingly relying on a patent license,
|
||||
and the Corresponding Source of the work is not available for anyone
|
||||
to copy, free of charge and under the terms of this License, through a
|
||||
publicly available network server or other readily accessible means,
|
||||
then you must either (1) cause the Corresponding Source to be so
|
||||
available, or (2) arrange to deprive yourself of the benefit of the
|
||||
patent license for this particular work, or (3) arrange, in a manner
|
||||
consistent with the requirements of this License, to extend the patent
|
||||
license to downstream recipients. "Knowingly relying" means you have
|
||||
actual knowledge that, but for the patent license, your conveying the
|
||||
covered work in a country, or your recipient's use of the covered work
|
||||
in a country, would infringe one or more identifiable patents in that
|
||||
country that you have reason to believe are valid.
|
||||
|
||||
If, pursuant to or in connection with a single transaction or
|
||||
arrangement, you convey, or propagate by procuring conveyance of, a
|
||||
covered work, and grant a patent license to some of the parties
|
||||
receiving the covered work authorizing them to use, propagate, modify
|
||||
or convey a specific copy of the covered work, then the patent license
|
||||
you grant is automatically extended to all recipients of the covered
|
||||
work and works based on it.
|
||||
|
||||
A patent license is "discriminatory" if it does not include within
|
||||
the scope of its coverage, prohibits the exercise of, or is
|
||||
conditioned on the non-exercise of one or more of the rights that are
|
||||
specifically granted under this License. You may not convey a covered
|
||||
work if you are a party to an arrangement with a third party that is
|
||||
in the business of distributing software, under which you make payment
|
||||
to the third party based on the extent of your activity of conveying
|
||||
the work, and under which the third party grants, to any of the
|
||||
parties who would receive the covered work from you, a discriminatory
|
||||
patent license (a) in connection with copies of the covered work
|
||||
conveyed by you (or copies made from those copies), or (b) primarily
|
||||
for and in connection with specific products or compilations that
|
||||
contain the covered work, unless you entered into that arrangement,
|
||||
or that patent license was granted, prior to 28 March 2007.
|
||||
|
||||
Nothing in this License shall be construed as excluding or limiting
|
||||
any implied license or other defenses to infringement that may
|
||||
otherwise be available to you under applicable patent law.
|
||||
|
||||
12. No Surrender of Others' Freedom.
|
||||
|
||||
If conditions are imposed on you (whether by court order, agreement or
|
||||
otherwise) that contradict the conditions of this License, they do not
|
||||
excuse you from the conditions of this License. If you cannot convey a
|
||||
covered work so as to satisfy simultaneously your obligations under this
|
||||
License and any other pertinent obligations, then as a consequence you may
|
||||
not convey it at all. For example, if you agree to terms that obligate you
|
||||
to collect a royalty for further conveying from those to whom you convey
|
||||
the Program, the only way you could satisfy both those terms and this
|
||||
License would be to refrain entirely from conveying the Program.
|
||||
|
||||
13. Use with the GNU Affero General Public License.
|
||||
|
||||
Notwithstanding any other provision of this License, you have
|
||||
permission to link or combine any covered work with a work licensed
|
||||
under version 3 of the GNU Affero General Public License into a single
|
||||
combined work, and to convey the resulting work. The terms of this
|
||||
License will continue to apply to the part which is the covered work,
|
||||
but the special requirements of the GNU Affero General Public License,
|
||||
section 13, concerning interaction through a network will apply to the
|
||||
combination as such.
|
||||
|
||||
14. Revised Versions of this License.
|
||||
|
||||
The Free Software Foundation may publish revised and/or new versions of
|
||||
the GNU General Public License from time to time. Such new versions will
|
||||
be similar in spirit to the present version, but may differ in detail to
|
||||
address new problems or concerns.
|
||||
|
||||
Each version is given a distinguishing version number. If the
|
||||
Program specifies that a certain numbered version of the GNU General
|
||||
Public License "or any later version" applies to it, you have the
|
||||
option of following the terms and conditions either of that numbered
|
||||
version or of any later version published by the Free Software
|
||||
Foundation. If the Program does not specify a version number of the
|
||||
GNU General Public License, you may choose any version ever published
|
||||
by the Free Software Foundation.
|
||||
|
||||
If the Program specifies that a proxy can decide which future
|
||||
versions of the GNU General Public License can be used, that proxy's
|
||||
public statement of acceptance of a version permanently authorizes you
|
||||
to choose that version for the Program.
|
||||
|
||||
Later license versions may give you additional or different
|
||||
permissions. However, no additional obligations are imposed on any
|
||||
author or copyright holder as a result of your choosing to follow a
|
||||
later version.
|
||||
|
||||
15. Disclaimer of Warranty.
|
||||
|
||||
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
|
||||
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
|
||||
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
|
||||
OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
|
||||
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
|
||||
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
|
||||
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
|
||||
|
||||
16. Limitation of Liability.
|
||||
|
||||
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
|
||||
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
|
||||
THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
|
||||
GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
|
||||
USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
|
||||
DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
|
||||
PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
|
||||
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGES.
|
||||
|
||||
17. Interpretation of Sections 15 and 16.
|
||||
|
||||
If the disclaimer of warranty and limitation of liability provided
|
||||
above cannot be given local legal effect according to their terms,
|
||||
reviewing courts shall apply local law that most closely approximates
|
||||
an absolute waiver of all civil liability in connection with the
|
||||
Program, unless a warranty or assumption of liability accompanies a
|
||||
copy of the Program in return for a fee.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
How to Apply These Terms to Your New Programs
|
||||
|
||||
If you develop a new program, and you want it to be of the greatest
|
||||
possible use to the public, the best way to achieve this is to make it
|
||||
free software which everyone can redistribute and change under these terms.
|
||||
|
||||
To do so, attach the following notices to the program. It is safest
|
||||
to attach them to the start of each source file to most effectively
|
||||
state the exclusion of warranty; and each file should have at least
|
||||
the "copyright" line and a pointer to where the full notice is found.
|
||||
|
||||
<one line to give the program's name and a brief idea of what it does.>
|
||||
Copyright (C) <year> <name of author>
|
||||
|
||||
This program 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
|
||||
(at your option) any later version.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
Also add information on how to contact you by electronic and paper mail.
|
||||
|
||||
If the program does terminal interaction, make it output a short
|
||||
notice like this when it starts in an interactive mode:
|
||||
|
||||
<program> Copyright (C) <year> <name of author>
|
||||
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
|
||||
This is free software, and you are welcome to redistribute it
|
||||
under certain conditions; type `show c' for details.
|
||||
|
||||
The hypothetical commands `show w' and `show c' should show the appropriate
|
||||
parts of the General Public License. Of course, your program's commands
|
||||
might be different; for a GUI interface, you would use an "about box".
|
||||
|
||||
You should also get your employer (if you work as a programmer) or school,
|
||||
if any, to sign a "copyright disclaimer" for the program, if necessary.
|
||||
For more information on this, and how to apply and follow the GNU GPL, see
|
||||
<http://www.gnu.org/licenses/>.
|
||||
|
||||
The GNU General Public License does not permit incorporating your program
|
||||
into proprietary programs. If your program is a subroutine library, you
|
||||
may consider it more useful to permit linking proprietary applications with
|
||||
the library. If this is what you want to do, use the GNU Lesser General
|
||||
Public License instead of this License. But first, please read
|
||||
<http://www.gnu.org/philosophy/why-not-lgpl.html>.
|
||||
151
INSTALL.md
151
INSTALL.md
|
|
@ -1,151 +0,0 @@
|
|||
# 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.
|
||||
|
||||
55
README.md
55
README.md
|
|
@ -1,13 +1,50 @@
|
|||
<!-- _____ _ _ __ >
|
||||
< / ____| | | |/ / >
|
||||
< ___ _ __ ___ _ __ | | __ | | ' / >
|
||||
< / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < >
|
||||
< | (_) | |_) | __/ | | | |__| | |__| | . \ >
|
||||
< \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ >
|
||||
< | | >
|
||||
< |_| >
|
||||
< >
|
||||
< Copyright 2022 Mattia Montanari, University of Oxford >
|
||||
< >
|
||||
< This program 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. You should have received a copy >
|
||||
< of the GNU General Public License along with this program. If not, visit >
|
||||
< >
|
||||
< https://www.gnu.org/licenses/ >
|
||||
< >
|
||||
< This program 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 GNU General Public License for details. -->
|
||||
|
||||
# Hello!
|
||||
|
||||
This is a simple and reliable C implementation of the Gilbert-Johnson-Keerthi (GJK) algorithm, [docs and details are available here](https://www.mattiamontanari.com/opengjk/).
|
||||
# Get started
|
||||
|
||||
All contributes are all welcome. For instance you could add:
|
||||
- Support for other shapes: quadrics and splines (easy)
|
||||
- More python examples and test (easy)
|
||||
- EPA algorithm (hard)
|
||||
If you have some basic tools installed (git, compiler and cmake) clone this repo:
|
||||
|
||||
> openGJK, Copyright (c) 2018-2021
|
||||
>
|
||||
> Department of Engineering Science. University of Oxford, UK.
|
||||
```
|
||||
git clone
|
||||
```
|
||||
|
||||
followed by these commands:
|
||||
|
||||
```
|
||||
cmake -E make_directory build
|
||||
cmake -E chdir build cmake -DRUN_UNITESTS=ON -DCMAKE_BUILD_TYPE=Release ..
|
||||
cmake --build build
|
||||
cmake -E chdir build/src/examples/c ./example_lib_opengjk_ce
|
||||
cmake -E chdir "build/test" ctest --build-config Release
|
||||
```
|
||||
|
||||
If you get no errors, the successfull output is:
|
||||
> `Distance between bodies 3.653650`.
|
||||
However, if you do get an error - any error - please file a bug! Support requests are welcome too.
|
||||
|
||||
# Beyond getting started
|
||||
|
||||
With the commands above you have built a demo example tha invokes the openGJK library. The library is statically linked and the distance between two bodies is computed and returned.
|
||||
|
||||
To learn how to use this library in your project the best place to start is the demo. Look at `main.c` and the other examples. In `examples/c/CMakeLists.txt` you can find how simple is to link using CMake.
|
||||
|
|
@ -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 srcdir and builddir in include path to save typing ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY} in every subdir
|
||||
set(CMAKE_INCLUDE_CURRENT_DIR ON)
|
||||
|
||||
# Put the include dirs which are in the source or build tree
|
||||
# before all other include dirs, so the headers in the sources
|
||||
# are prefered over the already installed ones
|
||||
# since cmake 2.4.1
|
||||
set(CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE ON)
|
||||
|
||||
# Use colored output
|
||||
set(CMAKE_COLOR_MAKEFILE ON)
|
||||
|
||||
# Create the compile command database for clang by default
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
# Always build with -fPIC
|
||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
||||
|
||||
# Avoid source tree pollution
|
||||
set(CMAKE_DISABLE_SOURCE_CHANGES ON)
|
||||
set(CMAKE_DISABLE_IN_SOURCE_BUILD ON)
|
||||
|
|
@ -1,44 +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 - 2020 #
|
||||
# http://iel.eng.ox.ac.uk/?page_id=504 #
|
||||
# #
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
|
||||
option(WITH_STATIC_LIB "Build static lib" OFF)
|
||||
option(WITH_EXAMPLES "Build C example" ON)
|
||||
|
||||
# Default build type
|
||||
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Release")
|
||||
|
||||
# APPLY USER OPTIONS
|
||||
IF (WITH_STATIC_LIB)
|
||||
set(BUILD_STATIC_LIB ON)
|
||||
ENDIF (WITH_STATIC_LIB)
|
||||
|
||||
# FEEDBACK
|
||||
message(STATUS " Build static lib (ON): " ${WITH_STATIC_LIB})
|
||||
message(STATUS " Build C examples (ON): " ${WITH_EXAMPLES})
|
||||
|
|
@ -1,22 +0,0 @@
|
|||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
1. Redistributions of source code must retain the copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
2. Redistributions in binary form must reproduce the copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
3. The name of the author may not be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
|
||||
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
|
||||
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
|
||||
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
|
||||
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
|
@ -1,109 +0,0 @@
|
|||
include(AddCCompilerFlag)
|
||||
include(CheckCCompilerFlagSSP)
|
||||
|
||||
if (UNIX)
|
||||
#
|
||||
# Check for -Werror turned on if possible
|
||||
#
|
||||
# This will prevent that compiler flags are detected incorrectly.
|
||||
#
|
||||
check_c_compiler_flag("-Werror" REQUIRED_FLAGS_WERROR)
|
||||
if (REQUIRED_FLAGS_WERROR)
|
||||
set(CMAKE_REQUIRED_FLAGS "-Werror")
|
||||
|
||||
if (PICKY_DEVELOPER)
|
||||
list(APPEND SUPPORTED_COMPILER_FLAGS "-Werror")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
add_c_compiler_flag("-std=gnu99" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wpedantic" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wall" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wshadow" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wmissing-prototypes" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wcast-align" SUPPORTED_COMPILER_FLAGS)
|
||||
#add_c_compiler_flag("-Wcast-qual" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=address" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wstrict-prototypes" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=strict-prototypes" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wwrite-strings" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=write-strings" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror-implicit-function-declaration" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wpointer-arith" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=pointer-arith" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wdeclaration-after-statement" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=declaration-after-statement" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wreturn-type" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=return-type" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wuninitialized" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=uninitialized" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wimplicit-fallthrough" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=strict-overflow" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wstrict-overflow=2" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wno-format-zero-length" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wmissing-field-initializers" SUPPORTED_COMPILER_FLAGS)
|
||||
|
||||
check_c_compiler_flag("-Wformat" REQUIRED_FLAGS_WFORMAT)
|
||||
if (REQUIRED_FLAGS_WFORMAT)
|
||||
list(APPEND SUPPORTED_COMPILER_FLAGS "-Wformat")
|
||||
set(CMAKE_REQUIRED_FLAGS "${CMAKE_REQUIRED_FLAGS} -Wformat")
|
||||
endif()
|
||||
add_c_compiler_flag("-Wformat-security" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Werror=format-security" SUPPORTED_COMPILER_FLAGS)
|
||||
|
||||
# Allow zero for a variadic macro argument
|
||||
string(TOLOWER "${CMAKE_C_COMPILER_ID}" _C_COMPILER_ID)
|
||||
if ("${_C_COMPILER_ID}" STREQUAL "clang")
|
||||
add_c_compiler_flag("-Wno-gnu-zero-variadic-macro-arguments" SUPPORTED_COMPILER_FLAGS)
|
||||
endif()
|
||||
|
||||
add_c_compiler_flag("-fno-common" SUPPORTED_COMPILER_FLAGS)
|
||||
|
||||
if (CMAKE_BUILD_TYPE)
|
||||
string(TOLOWER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_LOWER)
|
||||
if (CMAKE_BUILD_TYPE_LOWER MATCHES (release|relwithdebinfo|minsizerel))
|
||||
add_c_compiler_flag("-Wp,-D_FORTIFY_SOURCE=2" SUPPORTED_COMPILER_FLAGS)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
check_c_compiler_flag_ssp("-fstack-protector-strong" WITH_STACK_PROTECTOR_STRONG)
|
||||
if (WITH_STACK_PROTECTOR_STRONG)
|
||||
list(APPEND SUPPORTED_COMPILER_FLAGS "-fstack-protector-strong")
|
||||
# This is needed as Solaris has a seperate libssp
|
||||
if (SOLARIS)
|
||||
list(APPEND SUPPORTED_LINKER_FLAGS "-fstack-protector-strong")
|
||||
endif()
|
||||
else (WITH_STACK_PROTECTOR_STRONG)
|
||||
check_c_compiler_flag_ssp("-fstack-protector" WITH_STACK_PROTECTOR)
|
||||
if (WITH_STACK_PROTECTOR)
|
||||
list(APPEND SUPPORTED_COMPILER_FLAGS "-fstack-protector")
|
||||
# This is needed as Solaris has a seperate libssp
|
||||
if (SOLARIS)
|
||||
list(APPEND SUPPORTED_LINKER_FLAGS "-fstack-protector")
|
||||
endif()
|
||||
endif()
|
||||
endif (WITH_STACK_PROTECTOR_STRONG)
|
||||
|
||||
check_c_compiler_flag_ssp("-fstack-clash-protection" WITH_STACK_CLASH_PROTECTION)
|
||||
if (WITH_STACK_CLASH_PROTECTION)
|
||||
list(APPEND SUPPORTED_COMPILER_FLAGS "-fstack-clash-protection")
|
||||
endif()
|
||||
|
||||
if (PICKY_DEVELOPER)
|
||||
add_c_compiler_flag("-Wno-error=deprecated-declarations" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("-Wno-error=tautological-compare" SUPPORTED_COMPILER_FLAGS)
|
||||
endif()
|
||||
|
||||
# Unset CMAKE_REQUIRED_FLAGS
|
||||
unset(CMAKE_REQUIRED_FLAGS)
|
||||
endif()
|
||||
|
||||
if (MSVC)
|
||||
add_c_compiler_flag("/D _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES=1" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("/D _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES_COUNT=1" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("/D _CRT_NONSTDC_NO_WARNINGS=1" SUPPORTED_COMPILER_FLAGS)
|
||||
add_c_compiler_flag("/D _CRT_SECURE_NO_WARNINGS=1" SUPPORTED_COMPILER_FLAGS)
|
||||
endif()
|
||||
|
||||
set(DEFAULT_C_COMPILE_FLAGS ${SUPPORTED_COMPILER_FLAGS} CACHE INTERNAL "Default C Compiler Flags" FORCE)
|
||||
set(DEFAULT_LINK_FLAGS ${SUPPORTED_LINKER_FLAGS} CACHE INTERNAL "Default C Linker Flags" FORCE)
|
||||
|
|
@ -1,103 +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 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 #
|
||||
# #
|
||||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
|
||||
|
||||
# PLATFORM-SPECIFIC SETTING
|
||||
if (UNIX)
|
||||
find_library(M_LIB m)
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -lm")
|
||||
else ()
|
||||
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
|
||||
endif ()
|
||||
|
||||
if ("${CMAKE_C_COMPILER_ID}" STREQUAL "GNU")
|
||||
# using GCC
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -Werror")
|
||||
|
||||
add_compile_options(-static-libgcc -static-libstdc++ )
|
||||
add_definitions(-DMT)
|
||||
|
||||
elseif ("${CMAKE_C_COMPILER_ID}" STREQUAL "MSVC")
|
||||
|
||||
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()
|
||||
|
||||
if (UNIX AND NOT WIN32)
|
||||
|
||||
# Activate with: -DCMAKE_BUILD_TYPE=Debug
|
||||
set(CMAKE_C_FLAGS_DEBUG "-g -DDEBUG -Wall -Wextra -Werror"
|
||||
CACHE STRING "Flags used by the C compiler during DEBUG builds.")
|
||||
|
||||
# Activate with: -DCMAKE_BUILD_TYPE=Release
|
||||
set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -finline-functions -Wextra -Werror"
|
||||
CACHE STRING "Flags used by the C compiler during RELEASE builds.")
|
||||
|
||||
# Activate with: -DCMAKE_BUILD_TYPE=Profiling
|
||||
set(CMAKE_C_FLAGS_PROFILING "-O0 -g -fprofile-arcs -ftest-coverage"
|
||||
CACHE STRING "Flags used by the C compiler during PROFILING builds.")
|
||||
set(CMAKE_CXX_FLAGS_PROFILING "-O0 -g -fprofile-arcs -ftest-coverage"
|
||||
CACHE STRING "Flags used by the CXX compiler during PROFILING builds.")
|
||||
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "-fprofile-arcs -ftest-coverage"
|
||||
CACHE STRING "Flags used by the linker during the creation of shared libraries during PROFILING builds.")
|
||||
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "-fprofile-arcs -ftest-coverage"
|
||||
CACHE STRING "Flags used by the linker during the creation of shared libraries during PROFILING builds.")
|
||||
set(CMAKE_EXEC_LINKER_FLAGS_PROFILING "-fprofile-arcs -ftest-coverage"
|
||||
CACHE STRING "Flags used by the linker during PROFILING builds.")
|
||||
|
||||
# Activate with: -DCMAKE_BUILD_TYPE=AddressSanitizer
|
||||
set(CMAKE_C_FLAGS_ADDRESSSANITIZER "-g -O1 -fsanitize=address -fno-omit-frame-pointer"
|
||||
CACHE STRING "Flags used by the C compiler during ADDRESSSANITIZER builds.")
|
||||
set(CMAKE_CXX_FLAGS_ADDRESSSANITIZER "-g -O1 -fsanitize=address -fno-omit-frame-pointer"
|
||||
CACHE STRING "Flags used by the CXX compiler during ADDRESSSANITIZER builds.")
|
||||
set(CMAKE_SHARED_LINKER_FLAGS_ADDRESSSANITIZER "-fsanitize=address"
|
||||
CACHE STRING "Flags used by the linker during the creation of shared libraries during ADDRESSSANITIZER builds.")
|
||||
set(CMAKE_MODULE_LINKER_FLAGS_ADDRESSSANITIZER "-fsanitize=address"
|
||||
CACHE STRING "Flags used by the linker during the creation of shared libraries during ADDRESSSANITIZER builds.")
|
||||
set(CMAKE_EXEC_LINKER_FLAGS_ADDRESSSANITIZER "-fsanitize=address"
|
||||
CACHE STRING "Flags used by the linker during ADDRESSSANITIZER builds.")
|
||||
|
||||
# Activate with: -DCMAKE_BUILD_TYPE=MemorySanitizer
|
||||
set(CMAKE_C_FLAGS_MEMORYSANITIZER "-g -O2 -fsanitize=memory -fsanitize-memory-track-origins=2 -fno-omit-frame-pointer"
|
||||
CACHE STRING "Flags used by the C compiler during MEMORYSANITIZER builds.")
|
||||
set(CMAKE_CXX_FLAGS_MEMORYSANITIZER "-g -O2 -fsanitize=memory -fsanitize-memory-track-origins=2 -fno-omit-frame-pointer"
|
||||
CACHE STRING "Flags used by the CXX compiler during MEMORYSANITIZER builds.")
|
||||
set(CMAKE_SHARED_LINKER_FLAGS_MEMORYSANITIZER "-fsanitize=memory"
|
||||
CACHE STRING "Flags used by the linker during the creation of shared libraries during MEMORYSANITIZER builds.")
|
||||
set(CMAKE_MODULE_LINKER_FLAGS_MEMORYSANITIZER "-fsanitize=memory"
|
||||
CACHE STRING "Flags used by the linker during the creation of shared libraries during MEMORYSANITIZER builds.")
|
||||
set(CMAKE_EXEC_LINKER_FLAGS_MEMORYSANITIZER "-fsanitize=memory"
|
||||
CACHE STRING "Flags used by the linker during MEMORYSANITIZER builds.")
|
||||
|
||||
endif()
|
||||
|
|
@ -1,149 +0,0 @@
|
|||
include(CheckIncludeFile)
|
||||
include(CheckSymbolExists)
|
||||
include(CheckFunctionExists)
|
||||
include(CheckLibraryExists)
|
||||
include(CheckTypeSize)
|
||||
include(CheckCXXSourceCompiles)
|
||||
include(CheckStructHasMember)
|
||||
include(TestBigEndian)
|
||||
|
||||
set(PACKAGE ${PROJECT_NAME})
|
||||
set(VERSION ${PROJECT_VERSION})
|
||||
set(DATADIR ${DATA_INSTALL_DIR})
|
||||
set(LIBDIR ${CMAKE_INSTALL_LIBDIR})
|
||||
set(PLUGINDIR "${PLUGIN_INSTALL_DIR}-${LIBRARY_SOVERSION}")
|
||||
set(SYSCONFDIR ${SYSCONF_INSTALL_DIR})
|
||||
|
||||
set(BINARYDIR ${CMAKE_BINARY_DIR})
|
||||
set(SOURCEDIR ${CMAKE_SOURCE_DIR})
|
||||
|
||||
function(COMPILER_DUMPVERSION _OUTPUT_VERSION)
|
||||
# Remove whitespaces from the argument.
|
||||
# This is needed for CC="ccache gcc" cmake ..
|
||||
string(REPLACE " " "" _C_COMPILER_ARG "${CMAKE_C_COMPILER_ARG1}")
|
||||
|
||||
execute_process(
|
||||
COMMAND
|
||||
${CMAKE_C_COMPILER} ${_C_COMPILER_ARG} -dumpversion
|
||||
OUTPUT_VARIABLE _COMPILER_VERSION
|
||||
)
|
||||
|
||||
string(REGEX REPLACE "([0-9])\\.([0-9])(\\.[0-9])?" "\\1\\2"
|
||||
_COMPILER_VERSION ${_COMPILER_VERSION})
|
||||
|
||||
set(${_OUTPUT_VERSION} ${_COMPILER_VERSION} PARENT_SCOPE)
|
||||
endfunction()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCC AND NOT MINGW)
|
||||
compiler_dumpversion(GNUCC_VERSION)
|
||||
if (NOT GNUCC_VERSION EQUAL 34)
|
||||
check_c_compiler_flag("-fvisibility=hidden" WITH_VISIBILITY_HIDDEN)
|
||||
endif (NOT GNUCC_VERSION EQUAL 34)
|
||||
endif(CMAKE_COMPILER_IS_GNUCC AND NOT MINGW)
|
||||
|
||||
# DEFINITIONS
|
||||
if (SOLARIS)
|
||||
add_definitions(-D__EXTENSIONS__)
|
||||
endif (SOLARIS)
|
||||
|
||||
# HEADER FILES
|
||||
check_include_file(assert.h HAVE_ASSERT_H)
|
||||
check_include_file(inttypes.h HAVE_INTTYPES_H)
|
||||
check_include_file(io.h HAVE_IO_H)
|
||||
check_include_file(malloc.h HAVE_MALLOC_H)
|
||||
check_include_file(memory.h HAVE_MEMORY_H)
|
||||
check_include_file(setjmp.h HAVE_SETJMP_H)
|
||||
check_include_file(signal.h HAVE_SIGNAL_H)
|
||||
check_include_file(stdarg.h HAVE_STDARG_H)
|
||||
check_include_file(stddef.h HAVE_STDDEF_H)
|
||||
check_include_file(stdint.h HAVE_STDINT_H)
|
||||
check_include_file(stdio.h HAVE_STDIO_H)
|
||||
check_include_file(stdlib.h HAVE_STDLIB_H)
|
||||
check_include_file(string.h HAVE_STRING_H)
|
||||
check_include_file(strings.h HAVE_STRINGS_H)
|
||||
check_include_file(sys/stat.h HAVE_SYS_STAT_H)
|
||||
check_include_file(sys/types.h HAVE_SYS_TYPES_H)
|
||||
check_include_file(time.h HAVE_TIME_H)
|
||||
check_include_file(unistd.h HAVE_UNISTD_H)
|
||||
|
||||
if (HAVE_TIME_H)
|
||||
check_struct_has_member("struct timespec" tv_sec "time.h" HAVE_STRUCT_TIMESPEC)
|
||||
endif (HAVE_TIME_H)
|
||||
|
||||
# FUNCTIONS
|
||||
check_function_exists(calloc HAVE_CALLOC)
|
||||
check_function_exists(exit HAVE_EXIT)
|
||||
check_function_exists(fprintf HAVE_FPRINTF)
|
||||
check_function_exists(free HAVE_FREE)
|
||||
check_function_exists(longjmp HAVE_LONGJMP)
|
||||
check_function_exists(siglongjmp HAVE_SIGLONGJMP)
|
||||
check_function_exists(malloc HAVE_MALLOC)
|
||||
check_function_exists(memcpy HAVE_MEMCPY)
|
||||
check_function_exists(memset HAVE_MEMSET)
|
||||
check_function_exists(printf HAVE_PRINTF)
|
||||
check_function_exists(setjmp HAVE_SETJMP)
|
||||
check_function_exists(signal HAVE_SIGNAL)
|
||||
check_function_exists(strsignal HAVE_STRSIGNAL)
|
||||
check_function_exists(strcmp HAVE_STRCMP)
|
||||
check_function_exists(clock_gettime HAVE_CLOCK_GETTIME)
|
||||
|
||||
if (WIN32)
|
||||
check_function_exists(_vsnprintf_s HAVE__VSNPRINTF_S)
|
||||
check_function_exists(_vsnprintf HAVE__VSNPRINTF)
|
||||
check_function_exists(_snprintf HAVE__SNPRINTF)
|
||||
check_function_exists(_snprintf_s HAVE__SNPRINTF_S)
|
||||
check_symbol_exists(snprintf stdio.h HAVE_SNPRINTF)
|
||||
check_symbol_exists(vsnprintf stdio.h HAVE_VSNPRINTF)
|
||||
else (WIN32)
|
||||
check_function_exists(sprintf HAVE_SNPRINTF)
|
||||
check_function_exists(vsnprintf HAVE_VSNPRINTF)
|
||||
endif (WIN32)
|
||||
|
||||
find_library(RT_LIBRARY rt)
|
||||
if (RT_LIBRARY AND NOT LINUX AND NOT ANDROID)
|
||||
set(CMOCKA_REQUIRED_LIBRARIES ${RT_LIBRARY} CACHE INTERNAL "cmocka required system libraries")
|
||||
endif ()
|
||||
|
||||
# OPTIONS
|
||||
check_c_source_compiles("
|
||||
__thread int tls;
|
||||
|
||||
int main(void) {
|
||||
return 0;
|
||||
}" HAVE_GCC_THREAD_LOCAL_STORAGE)
|
||||
|
||||
if (WIN32)
|
||||
check_c_source_compiles("
|
||||
__declspec(thread) int tls;
|
||||
|
||||
int main(void) {
|
||||
return 0;
|
||||
}" HAVE_MSVC_THREAD_LOCAL_STORAGE)
|
||||
endif(WIN32)
|
||||
|
||||
if (HAVE_TIME_H AND HAVE_STRUCT_TIMESPEC AND HAVE_CLOCK_GETTIME)
|
||||
if (RT_LIBRARY)
|
||||
set(CMAKE_REQUIRED_LIBRARIES ${RT_LIBRARY})
|
||||
endif()
|
||||
|
||||
check_c_source_compiles("
|
||||
#include <time.h>
|
||||
|
||||
int main(void) {
|
||||
struct timespec ts;
|
||||
|
||||
clock_gettime(CLOCK_REALTIME, &ts);
|
||||
|
||||
return 0;
|
||||
}" HAVE_CLOCK_REALTIME)
|
||||
|
||||
# reset cmake requirements
|
||||
set(CMAKE_REQUIRED_INCLUDES)
|
||||
set(CMAKE_REQUIRED_LIBRARIES)
|
||||
endif ()
|
||||
|
||||
# ENDIAN
|
||||
if (NOT WIN32)
|
||||
set(WORDS_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P})
|
||||
test_big_endian(WORDS_BIGENDIAN)
|
||||
endif (NOT WIN32)
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
# Set system vars
|
||||
|
||||
if (CMAKE_SYSTEM_NAME MATCHES "Linux")
|
||||
set(LINUX TRUE)
|
||||
endif(CMAKE_SYSTEM_NAME MATCHES "Linux")
|
||||
|
||||
if (CMAKE_SYSTEM_NAME MATCHES "FreeBSD")
|
||||
set(FREEBSD TRUE)
|
||||
endif (CMAKE_SYSTEM_NAME MATCHES "FreeBSD")
|
||||
|
||||
if (CMAKE_SYSTEM_NAME MATCHES "OpenBSD")
|
||||
set(OPENBSD TRUE)
|
||||
endif (CMAKE_SYSTEM_NAME MATCHES "OpenBSD")
|
||||
|
||||
if (CMAKE_SYSTEM_NAME MATCHES "NetBSD")
|
||||
set(NETBSD TRUE)
|
||||
endif (CMAKE_SYSTEM_NAME MATCHES "NetBSD")
|
||||
|
||||
if (CMAKE_SYSTEM_NAME MATCHES "(Solaris|SunOS)")
|
||||
set(SOLARIS TRUE)
|
||||
endif (CMAKE_SYSTEM_NAME MATCHES "(Solaris|SunOS)")
|
||||
2491
doxygen/Doxyfile
2491
doxygen/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,67 +1,40 @@
|
|||
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
|
||||
# ##### # # # #
|
||||
# #### ##### ###### # # # # # # # #
|
||||
# # # # # # ## # # # # # #
|
||||
# # # # # ##### # # # # #### # ### #
|
||||
# # # ##### # # # # # # # # # # #
|
||||
# # # # # # ## # # # # # # #
|
||||
# #### # ###### # # ##### ##### # # #
|
||||
# #
|
||||
# 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 for this example after build
|
||||
add_custom_command(
|
||||
TARGET demo POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND} -E copy
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/userP.dat
|
||||
${CMAKE_CURRENT_BINARY_DIR}/userP.dat )
|
||||
add_custom_command(
|
||||
TARGET demo POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND} -E copy
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/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 ()
|
||||
|
||||
message(STATUS "Completed CMake setting for ${PROJECT_NAME}" )
|
||||
# _____ _ _ __ #
|
||||
# / ____| | | |/ / #
|
||||
# ___ _ __ ___ _ __ | | __ | | ' / #
|
||||
# / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < #
|
||||
# | (_) | |_) | __/ | | | |__| | |__| | . \ #
|
||||
# \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ #
|
||||
# | | #
|
||||
# |_| #
|
||||
# #
|
||||
# Copyright 2022 Mattia Montanari, University of Oxford #
|
||||
# #
|
||||
# This program 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. You should have received a copy #
|
||||
# of the GNU General Public License along with this program. If not, visit #
|
||||
# #
|
||||
# https://www.gnu.org/licenses/ #
|
||||
# #
|
||||
# This program 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 GNU General Public License for details. #
|
||||
|
||||
project(example_lib_opengjk_ce
|
||||
LANGUAGES C
|
||||
VERSION 1.0.0
|
||||
)
|
||||
|
||||
add_executable(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/main.c)
|
||||
target_link_libraries(${PROJECT_NAME} lib_opengjk_ce)
|
||||
|
||||
# Copy input files for this example after build
|
||||
add_custom_command(
|
||||
TARGET ${PROJECT_NAME} POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND} -E copy
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/userP.dat
|
||||
${CMAKE_CURRENT_BINARY_DIR}/userP.dat
|
||||
COMMAND ${CMAKE_COMMAND} -E copy
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/userQ.dat
|
||||
${CMAKE_CURRENT_BINARY_DIR}/userQ.dat
|
||||
)
|
||||
|
|
@ -1,178 +1,129 @@
|
|||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* 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(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(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);
|
||||
}
|
||||
// _____ _ _ __ //
|
||||
// / ____| | | |/ / //
|
||||
// ___ _ __ ___ _ __ | | __ | | ' / //
|
||||
// / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < //
|
||||
// | (_) | |_) | __/ | | | |__| | |__| | . \ //
|
||||
// \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ //
|
||||
// | | //
|
||||
// |_| //
|
||||
// //
|
||||
// Copyright 2022 Mattia Montanari, University of Oxford //
|
||||
// //
|
||||
// This program 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. You should have received a copy //
|
||||
// of the GNU General Public License along with this program. If not, visit //
|
||||
// //
|
||||
// https://www.gnu.org/licenses/ //
|
||||
// //
|
||||
// This program 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 GNU General Public License for details. //
|
||||
|
||||
/// @author Mattia Montanari
|
||||
/// @date July 2022
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "openGJK/openGJK.h"
|
||||
|
||||
#define fscanf_s fscanf
|
||||
|
||||
/// @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;
|
||||
}
|
||||
|
||||
fclose(fp);
|
||||
|
||||
*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. */
|
||||
gkSimplex s;
|
||||
/* Number of vertices defining body 1 and body 2, respectively. */
|
||||
int nvrtx1, nvrtx2;
|
||||
/* Structures of body 1 and body 2, respectively. */
|
||||
gkPolytope bd1;
|
||||
gkPolytope 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;
|
||||
|
||||
/* For importing openGJK this is Step 3: invoke the GJK procedure. */
|
||||
/* Compute squared distance using GJK algorithm. */
|
||||
dd = compute_minimum_distance(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,63 +1,63 @@
|
|||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* 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
|
||||
{
|
||||
|
||||
[DllImport("libopenGJKlib", EntryPoint="csFunction", CallingConvention = CallingConvention.StdCall)]
|
||||
|
||||
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
|
||||
{
|
||||
|
||||
[DllImport("libopenGJKlib", EntryPoint="csFunction", CallingConvention = CallingConvention.StdCall)]
|
||||
|
||||
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,62 @@
|
|||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% ##### # # # %
|
||||
% #### ##### ###### # # # # # # # %
|
||||
% # # # # # ## # # # # # %
|
||||
% # # # # ##### # # # # #### # ### %
|
||||
% # # ##### # # # # # # # # # # %
|
||||
% # # # # # ## # # # # # # %
|
||||
% #### # ###### # # ##### ##### # # %
|
||||
% %
|
||||
% 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');
|
||||
% _____ _ _ __ %
|
||||
% / ____| | | |/ / %
|
||||
% ___ _ __ ___ _ __ | | __ | | ' / %
|
||||
% / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < %
|
||||
% | (_) | |_) | __/ | | | |__| | |__| | . \ %
|
||||
% \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ %
|
||||
% | | %
|
||||
% |_| %
|
||||
% %
|
||||
% Copyright 2022 Mattia Montanari, University of Oxford %
|
||||
% %
|
||||
% This program 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. You should have received a copy %
|
||||
% of the GNU General Public License along with this program. If not, visit %
|
||||
% %
|
||||
% https://www.gnu.org/licenses/ %
|
||||
% %
|
||||
% This program 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 GNU General Public License for details. %
|
||||
|
||||
% 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,65 @@
|
|||
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
||||
% ##### # # # %
|
||||
% #### ##### ###### # # # # # # # %
|
||||
% # # # # # ## # # # # # %
|
||||
% # # # # ##### # # # # #### # ### %
|
||||
% # # ##### # # # # # # # # # # %
|
||||
% # # # # # ## # # # # # # %
|
||||
% #### # ###### # # ##### ##### # # %
|
||||
% %
|
||||
% 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('..','..','src','openGJK.c'),... % Source of openGJK
|
||||
'-largeArrayDims', ... % Support large arrays
|
||||
optflug, ... % Compiler flag for debug/optimisation
|
||||
fullfile('-I..','..','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
|
||||
% _____ _ _ __ %
|
||||
% / ____| | | |/ / %
|
||||
% ___ _ __ ___ _ __ | | __ | | ' / %
|
||||
% / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < %
|
||||
% | (_) | |_) | __/ | | | |__| | |__| | . \ %
|
||||
% \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ %
|
||||
% | | %
|
||||
% |_| %
|
||||
% %
|
||||
% Copyright 2022 Mattia Montanari, University of Oxford %
|
||||
% %
|
||||
% This program 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. You should have received a copy %
|
||||
% of the GNU General Public License along with this program. If not, visit %
|
||||
% %
|
||||
% https://www.gnu.org/licenses/ %
|
||||
% %
|
||||
% This program 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 GNU General Public License for details. %
|
||||
|
||||
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('..','..','src','openGJK.c'),... % Source of openGJK
|
||||
'-largeArrayDims', ... % Support large arrays
|
||||
optflug, ... % Compiler flag for debug/optimisation
|
||||
fullfile('-I..','..','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')
|
||||
|
|
@ -1,2 +1,2 @@
|
|||
#!/bin/bash
|
||||
#!/bin/bash
|
||||
g++ -Wall -fPIC -fopenmp -shared `python3 -m pybind11 --includes` -I ../../include -I/usr/include/eigen3 pyopenGJK.cpp ../../src/openGJK.c -o opengjkc`python3-config --extension-suffix`
|
||||
|
|
@ -3,23 +3,22 @@
|
|||
#include <pybind11/pybind11.h>
|
||||
namespace py = pybind11;
|
||||
|
||||
PYBIND11_MODULE(opengjkc, m)
|
||||
{
|
||||
PYBIND11_MODULE(opengjkc, m) {
|
||||
m.def("gjk",
|
||||
[](Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor>& arr1,
|
||||
Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor>& arr2)
|
||||
[](Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor> &arr1,
|
||||
Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor> &arr2)
|
||||
-> double {
|
||||
struct simplex s;
|
||||
struct bd bd1;
|
||||
struct bd bd2;
|
||||
bd1.numpoints = arr1.rows();
|
||||
std::vector<double*> arr1_rows(arr1.rows());
|
||||
std::vector<double *> arr1_rows(arr1.rows());
|
||||
for (int i = 0; i < arr1.rows(); ++i)
|
||||
arr1_rows[i] = arr1.row(i).data();
|
||||
bd1.coord = arr1_rows.data();
|
||||
|
||||
bd2.numpoints = arr2.rows();
|
||||
std::vector<double*> arr2_rows(arr2.rows());
|
||||
std::vector<double *> arr2_rows(arr2.rows());
|
||||
for (int i = 0; i < arr2.rows(); ++i)
|
||||
arr2_rows[i] = arr2.row(i).data();
|
||||
bd2.coord = arr2_rows.data();
|
||||
|
|
|
|||
|
|
@ -1,185 +1,185 @@
|
|||
import opengjkc as opengjk
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
import numpy as np
|
||||
import pytest
|
||||
#from IPython import embed
|
||||
|
||||
def settol():
|
||||
return 1e-12
|
||||
|
||||
def distance_point_to_line_3D(P1, P2, point):
|
||||
"""
|
||||
distance from point to line
|
||||
"""
|
||||
return np.linalg.norm(np.cross(P2-P1, P1-point))/np.linalg.norm(P2-P1)
|
||||
|
||||
|
||||
def distance_point_to_plane_3D(P1, P2, P3, point):
|
||||
"""
|
||||
Distance from point to plane
|
||||
"""
|
||||
return np.abs(np.dot(np.cross(P2-P1, P3-P1) /
|
||||
np.linalg.norm(np.cross(P2-P1, P3-P1)), point-P2))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0, -2])
|
||||
def test_line_point_distance(delta):
|
||||
line = np.array([[0.1, 0.2, 0.3], [0.5, 0.8, 0.7]], dtype=np.float64)
|
||||
point_on_line = line[0] + 0.27*(line[1]-line[0])
|
||||
normal = np.cross(line[0], line[1])
|
||||
point = point_on_line + delta * normal
|
||||
distance = opengjk.gjk(line, point)
|
||||
actual_distance = distance_point_to_line_3D(
|
||||
line[0], line[1], point)
|
||||
print(distance, actual_distance)
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0])
|
||||
def test_line_line_distance(delta):
|
||||
line = np.array([[-0.5, -0.7, -0.3], [1, 2, 3]], dtype=np.float64)
|
||||
point_on_line = line[0] + 0.38*(line[1]-line[0])
|
||||
normal = np.cross(line[0], line[1])
|
||||
point = point_on_line + delta * normal
|
||||
line_2 = np.array([point, [2, 5, 6]], dtype=np.float64)
|
||||
distance = opengjk.gjk(line, line_2)
|
||||
actual_distance = distance_point_to_line_3D(
|
||||
line[0], line[1], line_2[0])
|
||||
print(distance, actual_distance)
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0.1**(3*i) for i in range(6)])
|
||||
def test_tri_distance(delta):
|
||||
tri_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
|
||||
tri_2 = np.array([[1, delta, 0], [3, 1.2, 0], [
|
||||
1, 1, 0]], dtype=np.float64)
|
||||
P1 = tri_1[2]
|
||||
P2 = tri_1[1]
|
||||
point = tri_2[0]
|
||||
actual_distance = distance_point_to_line_3D(P1, P2, point)
|
||||
distance = opengjk.gjk(tri_1, tri_2)
|
||||
print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||
|
||||
#embed()
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0.1*0.1**(3*i) for i in range(6)])
|
||||
def test_quad_distance2d(delta):
|
||||
quad_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
|
||||
[1, 1, 0]], dtype=np.float64)
|
||||
quad_2 = np.array([[0, 1+delta, 0], [2, 2, 0],
|
||||
[2, 4, 0], [4, 4, 0]], dtype=np.float64)
|
||||
P1 = quad_1[2]
|
||||
P2 = quad_1[3]
|
||||
point = quad_2[0]
|
||||
actual_distance = distance_point_to_line_3D(P1, P2, point)
|
||||
distance = opengjk.gjk(quad_1, quad_2)
|
||||
print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [1*0.5**(3*i) for i in range(7)])
|
||||
def test_tetra_distance_3d(delta):
|
||||
tetra_1 = np.array([[0, 0, 0.2], [1, 0, 0.1], [0, 1, 0.3],
|
||||
[0, 0, 1]], dtype=np.float64)
|
||||
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
|
||||
[0.5, 0.3, -delta]], dtype=np.float64)
|
||||
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
|
||||
tetra_1[2], tetra_2[3])
|
||||
distance = opengjk.gjk(tetra_1, tetra_2)
|
||||
print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [(-1)**i*np.sqrt(2)*0.1**(3*i)
|
||||
for i in range(6)])
|
||||
def test_tetra_collision_3d(delta):
|
||||
tetra_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
|
||||
[0, 0, 1]], dtype=np.float64)
|
||||
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
|
||||
[0.5, 0.3, -delta]], dtype=np.float64)
|
||||
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
|
||||
tetra_1[2], tetra_2[3])
|
||||
distance = opengjk.gjk(tetra_1, tetra_2)
|
||||
|
||||
if delta < 0:
|
||||
assert(np.isclose(distance, 0, atol=settol()))
|
||||
else:
|
||||
print("Computed distance ", distance,
|
||||
"Actual distance ", actual_distance)
|
||||
assert(np.isclose(distance, actual_distance, atol=settol()))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0, -0.1, -0.49, -0.51])
|
||||
def test_hex_collision_3d(delta):
|
||||
hex_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0],
|
||||
[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]],
|
||||
dtype=np.float64)
|
||||
P0 = np.array([1.5+delta, 1.5+delta, 0.5], dtype=np.float64)
|
||||
P1 = np.array([2, 2, 1], dtype=np.float64)
|
||||
P2 = np.array([2, 1.25, 0.25], dtype=np.float64)
|
||||
P3 = P1 + P2 - P0
|
||||
quad_1 = np.array([P0, P1, P2, P3], dtype=np.float64)
|
||||
n = (np.cross(quad_1[1]-quad_1[0], quad_1[2]-quad_1[0]) /
|
||||
np.linalg.norm(
|
||||
np.cross(quad_1[1]-quad_1[0],
|
||||
quad_1[2]-quad_1[0])))
|
||||
quad_2 = quad_1 + n
|
||||
hex_2 = np.zeros((8, 3), dtype=np.float64)
|
||||
hex_2[:4, :] = quad_1
|
||||
hex_2[4:, :] = quad_2
|
||||
actual_distance = np.linalg.norm(
|
||||
np.array([1, 1, P0[2]], dtype=np.float64)-hex_2[0])
|
||||
distance = opengjk.gjk(hex_1, hex_2)
|
||||
|
||||
if P0[0] < 1:
|
||||
assert(np.isclose(distance, 0, atol=settol()))
|
||||
else:
|
||||
print("Computed distance ", distance,
|
||||
"Actual distance ", actual_distance)
|
||||
assert(np.isclose(distance, actual_distance, atol=settol()))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("c0", [0, 1, 2, 3])
|
||||
@pytest.mark.parametrize("c1", [0, 1, 2, 3])
|
||||
def test_cube_distance(c0, c1):
|
||||
cubes = [np.array([[-1, -1, -1], [1, -1, -1], [-1, 1, -1], [1, 1, -1],
|
||||
[-1, -1, 1], [1, -1, 1], [-1, 1, 1], [1, 1, 1]],
|
||||
dtype=np.float64)]
|
||||
|
||||
r = R.from_euler('z', 45, degrees=True)
|
||||
cubes.append(r.apply(cubes[0]))
|
||||
r = R.from_euler('y', np.arctan2(1.0, np.sqrt(2)))
|
||||
cubes.append(r.apply(cubes[1]))
|
||||
r = R.from_euler('y', 45, degrees=True)
|
||||
cubes.append(r.apply(cubes[0]))
|
||||
|
||||
dx = cubes[c0][:,0].max() - cubes[c1][:,0].min()
|
||||
cube0 = cubes[c0]
|
||||
|
||||
for delta in [1e8, 1.0, 1e-4, 1e-8, 1e-12]:
|
||||
cube1 = cubes[c1] + np.array([dx + delta, 0, 0])
|
||||
distance = opengjk.gjk(cube0, cube1)
|
||||
print(distance, delta)
|
||||
assert(np.isclose(distance, delta))
|
||||
|
||||
def test_random_objects():
|
||||
for i in range(1, 8):
|
||||
for j in range(1, 8):
|
||||
for k in range(1000):
|
||||
arr1 = np.random.rand(i, 3)
|
||||
arr2 = np.random.rand(j, 3)
|
||||
opengjk.gjk(arr1, arr2)
|
||||
|
||||
|
||||
def test_large_random_objects():
|
||||
for i in range(1, 8):
|
||||
for j in range(1, 8):
|
||||
for k in range(1000):
|
||||
arr1 = 10000.0*np.random.rand(i, 3)
|
||||
arr2 = 10000.0*np.random.rand(j, 3)
|
||||
opengjk.gjk(arr1, arr2)
|
||||
import opengjkc as opengjk
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
import numpy as np
|
||||
import pytest
|
||||
#from IPython import embed
|
||||
|
||||
def settol():
|
||||
return 1e-12
|
||||
|
||||
def distance_point_to_line_3D(P1, P2, point):
|
||||
"""
|
||||
distance from point to line
|
||||
"""
|
||||
return np.linalg.norm(np.cross(P2-P1, P1-point))/np.linalg.norm(P2-P1)
|
||||
|
||||
|
||||
def distance_point_to_plane_3D(P1, P2, P3, point):
|
||||
"""
|
||||
Distance from point to plane
|
||||
"""
|
||||
return np.abs(np.dot(np.cross(P2-P1, P3-P1) /
|
||||
np.linalg.norm(np.cross(P2-P1, P3-P1)), point-P2))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0, -2])
|
||||
def test_line_point_distance(delta):
|
||||
line = np.array([[0.1, 0.2, 0.3], [0.5, 0.8, 0.7]], dtype=np.float64)
|
||||
point_on_line = line[0] + 0.27*(line[1]-line[0])
|
||||
normal = np.cross(line[0], line[1])
|
||||
point = point_on_line + delta * normal
|
||||
distance = opengjk.gjk(line, point)
|
||||
actual_distance = distance_point_to_line_3D(
|
||||
line[0], line[1], point)
|
||||
print(distance, actual_distance)
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0])
|
||||
def test_line_line_distance(delta):
|
||||
line = np.array([[-0.5, -0.7, -0.3], [1, 2, 3]], dtype=np.float64)
|
||||
point_on_line = line[0] + 0.38*(line[1]-line[0])
|
||||
normal = np.cross(line[0], line[1])
|
||||
point = point_on_line + delta * normal
|
||||
line_2 = np.array([point, [2, 5, 6]], dtype=np.float64)
|
||||
distance = opengjk.gjk(line, line_2)
|
||||
actual_distance = distance_point_to_line_3D(
|
||||
line[0], line[1], line_2[0])
|
||||
print(distance, actual_distance)
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0.1**(3*i) for i in range(6)])
|
||||
def test_tri_distance(delta):
|
||||
tri_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
|
||||
tri_2 = np.array([[1, delta, 0], [3, 1.2, 0], [
|
||||
1, 1, 0]], dtype=np.float64)
|
||||
P1 = tri_1[2]
|
||||
P2 = tri_1[1]
|
||||
point = tri_2[0]
|
||||
actual_distance = distance_point_to_line_3D(P1, P2, point)
|
||||
distance = opengjk.gjk(tri_1, tri_2)
|
||||
print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||
|
||||
#embed()
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0.1*0.1**(3*i) for i in range(6)])
|
||||
def test_quad_distance2d(delta):
|
||||
quad_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
|
||||
[1, 1, 0]], dtype=np.float64)
|
||||
quad_2 = np.array([[0, 1+delta, 0], [2, 2, 0],
|
||||
[2, 4, 0], [4, 4, 0]], dtype=np.float64)
|
||||
P1 = quad_1[2]
|
||||
P2 = quad_1[3]
|
||||
point = quad_2[0]
|
||||
actual_distance = distance_point_to_line_3D(P1, P2, point)
|
||||
distance = opengjk.gjk(quad_1, quad_2)
|
||||
print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [1*0.5**(3*i) for i in range(7)])
|
||||
def test_tetra_distance_3d(delta):
|
||||
tetra_1 = np.array([[0, 0, 0.2], [1, 0, 0.1], [0, 1, 0.3],
|
||||
[0, 0, 1]], dtype=np.float64)
|
||||
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
|
||||
[0.5, 0.3, -delta]], dtype=np.float64)
|
||||
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
|
||||
tetra_1[2], tetra_2[3])
|
||||
distance = opengjk.gjk(tetra_1, tetra_2)
|
||||
print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||
|
||||
assert(np.isclose(distance, actual_distance, atol=settol() ))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [(-1)**i*np.sqrt(2)*0.1**(3*i)
|
||||
for i in range(6)])
|
||||
def test_tetra_collision_3d(delta):
|
||||
tetra_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
|
||||
[0, 0, 1]], dtype=np.float64)
|
||||
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
|
||||
[0.5, 0.3, -delta]], dtype=np.float64)
|
||||
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
|
||||
tetra_1[2], tetra_2[3])
|
||||
distance = opengjk.gjk(tetra_1, tetra_2)
|
||||
|
||||
if delta < 0:
|
||||
assert(np.isclose(distance, 0, atol=settol()))
|
||||
else:
|
||||
print("Computed distance ", distance,
|
||||
"Actual distance ", actual_distance)
|
||||
assert(np.isclose(distance, actual_distance, atol=settol()))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("delta", [0, -0.1, -0.49, -0.51])
|
||||
def test_hex_collision_3d(delta):
|
||||
hex_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0],
|
||||
[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]],
|
||||
dtype=np.float64)
|
||||
P0 = np.array([1.5+delta, 1.5+delta, 0.5], dtype=np.float64)
|
||||
P1 = np.array([2, 2, 1], dtype=np.float64)
|
||||
P2 = np.array([2, 1.25, 0.25], dtype=np.float64)
|
||||
P3 = P1 + P2 - P0
|
||||
quad_1 = np.array([P0, P1, P2, P3], dtype=np.float64)
|
||||
n = (np.cross(quad_1[1]-quad_1[0], quad_1[2]-quad_1[0]) /
|
||||
np.linalg.norm(
|
||||
np.cross(quad_1[1]-quad_1[0],
|
||||
quad_1[2]-quad_1[0])))
|
||||
quad_2 = quad_1 + n
|
||||
hex_2 = np.zeros((8, 3), dtype=np.float64)
|
||||
hex_2[:4, :] = quad_1
|
||||
hex_2[4:, :] = quad_2
|
||||
actual_distance = np.linalg.norm(
|
||||
np.array([1, 1, P0[2]], dtype=np.float64)-hex_2[0])
|
||||
distance = opengjk.gjk(hex_1, hex_2)
|
||||
|
||||
if P0[0] < 1:
|
||||
assert(np.isclose(distance, 0, atol=settol()))
|
||||
else:
|
||||
print("Computed distance ", distance,
|
||||
"Actual distance ", actual_distance)
|
||||
assert(np.isclose(distance, actual_distance, atol=settol()))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("c0", [0, 1, 2, 3])
|
||||
@pytest.mark.parametrize("c1", [0, 1, 2, 3])
|
||||
def test_cube_distance(c0, c1):
|
||||
cubes = [np.array([[-1, -1, -1], [1, -1, -1], [-1, 1, -1], [1, 1, -1],
|
||||
[-1, -1, 1], [1, -1, 1], [-1, 1, 1], [1, 1, 1]],
|
||||
dtype=np.float64)]
|
||||
|
||||
r = R.from_euler('z', 45, degrees=True)
|
||||
cubes.append(r.apply(cubes[0]))
|
||||
r = R.from_euler('y', np.arctan2(1.0, np.sqrt(2)))
|
||||
cubes.append(r.apply(cubes[1]))
|
||||
r = R.from_euler('y', 45, degrees=True)
|
||||
cubes.append(r.apply(cubes[0]))
|
||||
|
||||
dx = cubes[c0][:,0].max() - cubes[c1][:,0].min()
|
||||
cube0 = cubes[c0]
|
||||
|
||||
for delta in [1e8, 1.0, 1e-4, 1e-8, 1e-12]:
|
||||
cube1 = cubes[c1] + np.array([dx + delta, 0, 0])
|
||||
distance = opengjk.gjk(cube0, cube1)
|
||||
print(distance, delta)
|
||||
assert(np.isclose(distance, delta))
|
||||
|
||||
def test_random_objects():
|
||||
for i in range(1, 8):
|
||||
for j in range(1, 8):
|
||||
for k in range(1000):
|
||||
arr1 = np.random.rand(i, 3)
|
||||
arr2 = np.random.rand(j, 3)
|
||||
opengjk.gjk(arr1, arr2)
|
||||
|
||||
|
||||
def test_large_random_objects():
|
||||
for i in range(1, 8):
|
||||
for j in range(1, 8):
|
||||
for k in range(1000):
|
||||
arr1 = 10000.0*np.random.rand(i, 3)
|
||||
arr2 = 10000.0*np.random.rand(j, 3)
|
||||
opengjk.gjk(arr1, arr2)
|
||||
|
|
|
|||
|
|
@ -1,52 +1,55 @@
|
|||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* 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
|
||||
// _____ _ _ __ //
|
||||
// / ____| | | |/ / //
|
||||
// ___ _ __ ___ _ __ | | __ | | ' / //
|
||||
// / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < //
|
||||
// | (_) | |_) | __/ | | | |__| | |__| | . \ //
|
||||
// \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ //
|
||||
// | | //
|
||||
// |_| //
|
||||
// //
|
||||
// Copyright 2022 Mattia Montanari, University of Oxford //
|
||||
// //
|
||||
// This program 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. You should have received a copy //
|
||||
// of the GNU General Public License along with this program. If not, visit //
|
||||
// //
|
||||
// https://www.gnu.org/licenses/ //
|
||||
// //
|
||||
// This program 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 GNU General Public License for details. //
|
||||
|
||||
#ifndef OPENGJK_H__
|
||||
#define OPENGJK_H__
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/// @brief Use double as default precision
|
||||
#define gkFloat double
|
||||
|
||||
/// @brief Structure of a body
|
||||
typedef struct gkPolytope_ {
|
||||
int numpoints; // Number of points defining the body
|
||||
gkFloat s[3]; // Support mapping computed last
|
||||
gkFloat **coord; // Points' coordinates
|
||||
} gkPolytope;
|
||||
|
||||
/// @brief Structure of the simplex
|
||||
typedef struct gkSimplex_ {
|
||||
int nvrtx; // Number of simplex's vertices
|
||||
int wids[4]; // Label of the simplex's vertices
|
||||
gkFloat lambdas[4]; // Barycentric coordiantes for each vertex
|
||||
gkFloat vrtx[4][3]; // Coordinates of simplex's vertices
|
||||
} gkSimplex;
|
||||
|
||||
/// @brief Uses the GJK algorithm to compute the minimum distance between two bodies
|
||||
gkFloat compute_minimum_distance(const gkPolytope p_, const gkPolytope q_, gkSimplex *s_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // OPENGJK_H__
|
||||
|
|
|
|||
|
|
@ -0,0 +1,823 @@
|
|||
// _____ _ _ __ //
|
||||
// / ____| | | |/ / //
|
||||
// ___ _ __ ___ _ __ | | __ | | ' / //
|
||||
// / _ \| '_ \ / _ \ '_ \| | |_ |_ | | < //
|
||||
// | (_) | |_) | __/ | | | |__| | |__| | . \ //
|
||||
// \___/| .__/ \___|_| |_|\_____|\____/|_|\_\ //
|
||||
// | | //
|
||||
// |_| //
|
||||
// //
|
||||
// Copyright 2022 Mattia Montanari, University of Oxford //
|
||||
// //
|
||||
// This program 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. You should have received a copy //
|
||||
// of the GNU General Public License along with this program. If not, visit //
|
||||
// //
|
||||
// https://www.gnu.org/licenses/ //
|
||||
// //
|
||||
// This program 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 GNU General Public License for details. //
|
||||
|
||||
#include "openGJK/openGJK.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "math.h"
|
||||
|
||||
/* If instricuted, compile a mex function for Matlab. */
|
||||
#ifdef MATLAB_MEX_BUILD
|
||||
#include "mex.h"
|
||||
#else
|
||||
#define mexPrintf printf
|
||||
#endif
|
||||
|
||||
#define eps_rel22 1e-10
|
||||
#define eps_tot22 1e-12
|
||||
|
||||
#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 gkFloat determinant(const gkFloat *p, const gkFloat *q, const gkFloat *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 gkFloat *a, const gkFloat *b, gkFloat *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 gkFloat *p, const gkFloat *q, gkFloat *v) {
|
||||
gkFloat pq[3];
|
||||
gkFloat 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 gkFloat *p, const gkFloat *q, const gkFloat *r, gkFloat *v) {
|
||||
gkFloat n[3], pq[3], pr[3];
|
||||
gkFloat 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 gkFloat *p, const gkFloat *q) {
|
||||
gkFloat tmp = 0;
|
||||
|
||||
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 gkFloat *p, const gkFloat *q, const gkFloat *r) {
|
||||
gkFloat ntmp[3];
|
||||
gkFloat n[3], pq[3], pr[3];
|
||||
gkFloat 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);
|
||||
|
||||
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 gkFloat *p, const gkFloat *q, const gkFloat *r) {
|
||||
gkFloat n[3], pq[3], pr[3];
|
||||
gkFloat 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);
|
||||
|
||||
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(gkSimplex *s, gkFloat *v) {
|
||||
gkFloat *s1p = s->vrtx[1];
|
||||
gkFloat *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(gkSimplex *s, gkFloat *v) {
|
||||
gkFloat *s1p = s->vrtx[2];
|
||||
gkFloat *s2p = s->vrtx[1];
|
||||
gkFloat *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 v
|
||||
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(gkSimplex *s, gkFloat *v) {
|
||||
gkFloat s1[3], s2[3], s3[3], s4[3], s1s2[3], s1s3[3], s1s4[3];
|
||||
gkFloat 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;
|
||||
}
|
||||
|
||||
gkFloat 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(gkPolytope *body, const gkFloat *v) {
|
||||
gkFloat s, maxs;
|
||||
gkFloat *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(gkSimplex *s, gkFloat *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");
|
||||
}
|
||||
}
|
||||
|
||||
gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) {
|
||||
int k = 0; /**< Iteration counter */
|
||||
int i; /**< General purpose counter */
|
||||
int mk = 25; /**< Maximum number of iterations of the GJK algorithm */
|
||||
int absTestin;
|
||||
gkFloat norm2Wmax = 0;
|
||||
gkFloat tesnorm;
|
||||
gkFloat v[3]; /**< Search direction */
|
||||
gkFloat vminus[3]; /**< Search direction * -1 */
|
||||
gkFloat w[3]; /**< Vertex on CSO boundary given by the difference of support
|
||||
functions on both bodies */
|
||||
gkFloat eps_rel = eps_rel22; /**< Tolerance on relative */
|
||||
gkFloat eps_rel2 = eps_rel * eps_rel;
|
||||
gkFloat eps_tot = eps_tot22;
|
||||
gkFloat exeedtol_rel; /**< Test for 1st exit condition */
|
||||
int nullV = 0;
|
||||
|
||||
/* 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));
|
||||
if (exeedtol_rel <= (eps_rel * norm2(v)) || exeedtol_rel < eps_tot22) {
|
||||
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 MATLAB_MEX_BUILD
|
||||
/**
|
||||
* @brief Mex function for Matlab.
|
||||
*/
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
|
||||
gkFloat *inCoordsA;
|
||||
gkFloat *inCoordsB;
|
||||
size_t nCoordsA;
|
||||
size_t nCoordsB;
|
||||
int i;
|
||||
gkFloat *distance;
|
||||
int c = 3;
|
||||
int count = 0;
|
||||
gkFloat **arr1;
|
||||
gkFloat **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] = mxCreategkFloatMatrix(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 = (gkFloat **)mxMalloc(sizeof(gkFloat *) * (int)nCoordsA);
|
||||
arr2 = (gkFloat **)mxMalloc(sizeof(gkFloat *) * (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 */
|
||||
|
||||
gkPolytope bd1; /* Structure of body A */
|
||||
gkPolytope 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 */
|
||||
|
||||
gkSimplex s;
|
||||
s.nvrtx = 0;
|
||||
|
||||
/* Compute squared distance using GJK algorithm */
|
||||
distance[0] = gjk(bd1, bd2, &s);
|
||||
|
||||
mxFree(arr1);
|
||||
mxFree(arr2);
|
||||
}
|
||||
#endif
|
||||
#ifdef CS_MONO_BUILD
|
||||
/**
|
||||
* @brief Invoke this function from C# applications
|
||||
*/
|
||||
gkFloat csFunction(int nCoordsA, gkFloat *inCoordsA, int nCoordsB, gkFloat *inCoordsB) {
|
||||
gkFloat distance = 0;
|
||||
int i, j;
|
||||
|
||||
/*----------------------------------------------------------------*/
|
||||
/* POPULATE BODIES' STRUCTURES */
|
||||
|
||||
gkPolytope bd1; /* Structure of body A */
|
||||
gkPolytope bd2; /* Structure of body B */
|
||||
|
||||
/* Assign number of vertices to each body */
|
||||
bd1.numpoints = (int)nCoordsA;
|
||||
bd2.numpoints = (int)nCoordsB;
|
||||
|
||||
gkFloat **pinCoordsA = (gkFloat **)malloc(bd1.numpoints * sizeof(gkFloat *));
|
||||
for (i = 0; i < bd1.numpoints; i++) pinCoordsA[i] = (gkFloat *)malloc(3 * sizeof(gkFloat));
|
||||
|
||||
for (i = 0; i < 3; i++)
|
||||
for (j = 0; j < bd1.numpoints; j++) pinCoordsA[j][i] = inCoordsA[i * bd1.numpoints + j];
|
||||
|
||||
gkFloat **pinCoordsB = (gkFloat **)malloc(bd2.numpoints * sizeof(gkFloat *));
|
||||
for (i = 0; i < bd2.numpoints; i++) pinCoordsB[i] = (gkFloat *)malloc(3 * sizeof(gkFloat));
|
||||
|
||||
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 */
|
||||
gkSimplex s;
|
||||
|
||||
/* Initialise simplex as empty */
|
||||
s.nvrtx = 0;
|
||||
|
||||
/* Compute squared distance using GJK algorithm */
|
||||
distance = compute_minimum_distance(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;
|
||||
}
|
||||
#endif //CS_MONO_BUILD
|
||||
946
src/openGJK.c
946
src/openGJK.c
|
|
@ -1,946 +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 "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-10
|
||||
#define eps_tot22 1e-12
|
||||
|
||||
/* 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 v
|
||||
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 = 25; /**< 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;
|
||||
double exeedtol_rel; /**< Test 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));
|
||||
if ( exeedtol_rel <= (eps_rel * norm2(v)) || exeedtol_rel < eps_tot22) {
|
||||
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