ecker/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop'

Browse Source
This commit is contained in:
Daniel Chappuis 2018-04-30 23:02:03 +02:00
commit b542285744
2170 changed files with 288195 additions and 129673 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.codecov.yml Normal file
View File

@ -0,0 +1,3 @@
ignore:
- "usr/"
- "test/"

237
.travis.yml
View File

@ -1,7 +1,12 @@
language: cpp
install:
- if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then sudo apt-get update -qq; fi
- if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then sudo apt-get install valgrind; fi
matrix:
# Linux / GCC
# ----- Linux / GCC -----
include:
- os: linux
addons:
@ -9,50 +14,226 @@ matrix:
sources:
- ubuntu-toolchain-r-test
packages:
- g++-4.9
- g++-7
env:
- MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9"
- MATRIX_EVAL="CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Debug" DOUBLE_PRECISION="False"
# OS X / GCC
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9"
# Linux / Clang
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- llvm-toolchain-precise-3.6
packages:
- clang-3.6
- g++-7
env:
- MATRIX_EVAL="CC=clang-3.6 && CXX=clang++-3.6"
- MATRIX_EVAL="CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Release" DOUBLE_PRECISION="False"
# OS X / Clang
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
packages:
- g++-7
env:
- MATRIX_EVAL="CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Debug" DOUBLE_PRECISION="True"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
packages:
- g++-7
env:
- MATRIX_EVAL="CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Release" DOUBLE_PRECISION="True"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
packages:
- g++-7
env:
- MATRIX_EVAL="CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Debug" DOUBLE_PRECISION="False" LOGGER="True" Profiler="True"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
packages:
- g++-7
env:
- MATRIX_EVAL="CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Release" DOUBLE_PRECISION="False" LOGGER="True" Profiler="True"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
packages:
- g++-7
- lcov
env:
- MATRIX_EVAL="CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Release" DOUBLE_PRECISION="False" CODE_COVERAGE="True"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
packages:
- g++-7
- valgrind
env:
- MATRIX_EVAL="CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Debug" DOUBLE_PRECISION="False" VALGRIND="True"
# ----- OS X / GCC -----
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="brew update && brew install gcc && CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Debug" DOUBLE_PRECISION="False"
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="brew update && brew install gcc && CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Release" DOUBLE_PRECISION="False"
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="brew update && brew install gcc && CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Debug" DOUBLE_PRECISION="True"
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="brew update && brew install gcc && CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Release" DOUBLE_PRECISION="True"
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="brew update && brew install gcc && CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Debug" DOUBLE_PRECISION="True" LOGGER="True" PROFILER="True"
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="brew update && brew install gcc && CC=gcc-7 && CXX=g++-7" BUILD_TYPE="Release" DOUBLE_PRECISION="True" LOGGER="True" PROFILER="True"
# ----- Linux / Clang -----
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- llvm-toolchain-precise-3.8
packages:
- clang-3.8
- g++-7
env:
- MATRIX_EVAL="CC=clang-3.8 && CXX=clang++-3.8" BUILD_TYPE="Debug" DOUBLE_PRECISION="False"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- llvm-toolchain-precise-3.8
packages:
- clang-3.8
- g++-7
env:
- MATRIX_EVAL="CC=clang-3.8 && CXX=clang++-3.8" BUILD_TYPE="Release" DOUBLE_PRECISION="False"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- llvm-toolchain-precise-3.8
packages:
- clang-3.8
- g++-7
env:
- MATRIX_EVAL="CC=clang-3.8 && CXX=clang++-3.8" BUILD_TYPE="Debug" DOUBLE_PRECISION="True"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- llvm-toolchain-precise-3.8
packages:
- clang-3.8
- g++-7
env:
- MATRIX_EVAL="CC=clang-3.8 && CXX=clang++-3.8" BUILD_TYPE="Release" DOUBLE_PRECISION="True"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- llvm-toolchain-precise-3.8
packages:
- clang-3.8
- g++-7
env:
- MATRIX_EVAL="CC=clang-3.8 && CXX=clang++-3.8" BUILD_TYPE="Debug" DOUBLE_PRECISION="True" LOGGER="True" PROFILER="True"
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- llvm-toolchain-precise-3.8
packages:
- clang-3.8
- g++-7
env:
- MATRIX_EVAL="CC=clang-3.8 && CXX=clang++-3.8" BUILD_TYPE="Release" DOUBLE_PRECISION="True" LOGGER="True" PROFILER="True"
# ----- OS X / Clang -----
- os: osx
osx_image: xcode8
env:
- BUILD_TYPE="Debug" DOUBLE_PRECISION="False"
- os: osx
osx_image: xcode8
env:
- BUILD_TYPE="Release" DOUBLE_PRECISION="False"
- os: osx
osx_image: xcode8
env:
- BUILD_TYPE="Debug" DOUBLE_PRECISION="True"
- os: osx
osx_image: xcode8
env:
- BUILD_TYPE="Release" DOUBLE_PRECISION="True"
- os: osx
osx_image: xcode8
env:
- BUILD_TYPE="Debug" DOUBLE_PRECISION="True" LOGGER="True" PROFILER="True"
- os: osx
osx_image: xcode8
env:
- BUILD_TYPE="Release" DOUBLE_PRECISION="True" LOGGER="True" PROFILER="True"
before_install:
- eval "${MATRIX_EVAL}"
branches:
only:
- master
- develop
script:
- mkdir build_directory
- cd build_directory
# Build in debug mode with double precision
- cmake -G "Unix Makefiles" -DCMAKE_BUILD_TYPE=Debug —DDOUBLE_PRECISION_ENABLED=True -DCOMPILE_TESTS=True ../
- make && make test ARGS="-V"
# Build in debug mode with single precision
- cmake -G "Unix Makefiles" -DCMAKE_BUILD_TYPE=Debug —DDOUBLE_PRECISION_ENABLED=False -DCOMPILE_TESTS=True ../
- make && make test ARGS="-V"
# Build in release mode with double precision
- cmake -G "Unix Makefiles" -DCMAKE_BUILD_TYPE=Release —DDOUBLE_PRECISION_ENABLED=True -DCOMPILE_TESTS=True ../
- make && make test ARGS="-V"
# Build in release mode with single precision
- cmake -G "Unix Makefiles" -DCMAKE_BUILD_TYPE=Release —DDOUBLE_PRECISION_ENABLED=False -DCOMPILE_TESTS=True ../
- make && make test ARGS="-V"
- mkdir build_directory
- cd build_directory
- cmake -DCMAKE_BUILD_TYPE=${BUILD_TYPE} —DRP3D_DOUBLE_PRECISION_ENABLED=${DOUBLE_PRECISION} -DRP3D_COMPILE_TESTS=True -DRP3D_LOGS_ENABLE=${LOGGER} -DRP3D_PROFILING_ENABLED=${PROFILER} -DRP3D_CODE_COVERAGE_ENABLED=${CODE_COVERAGE} ../
- make && make test ARGS="-V"
- if [ "${VALGRIND}" == "True" ]; then
valgrind --leak-check=full --show-leak-kinds=all --track-origins=yes --verbose --error-exitcode=1 test/tests;
fi
after_success:
# Generate code coverage report
- if [ "${CODE_COVERAGE}" == "True" ]; then
bash <(curl -s https://codecov.io/bash) || echo "Codecov did not collect coverage reports";
fi

160
CHANGELOG.md Normal file
View File

@ -0,0 +1,160 @@
#Changelog
## Version 0.7.0 (April 30, 2018)
### Added
- Dedicated Sphere vs Capsule collision detection algorithm.
- Dedicated Capsule vs Capsule collision detection algorithm.
- Allow a Rigid Body to have zero mass (for static bodies for instance).
- Add single frame memory allocator for faster memory allocation in a frame.
- Make possible to have a profiler per DynamicsWorld instead of a unique one.
- Make possible to use your own allocation/deallocation methods instead of default malloc/free
- Make possible to display the AABB of the bodies in the testbed application.
- Add RigidBody::setInverseLocalInertiaTensor() method to directly set the inverse inertia tensor of a rigid body.
- More unit tests have been added
### Changed
- Use single-shot contact manifold computation instead of incremental across several frames.
- Replace the EPA narrow-phase collision detection with SAT algorithm.
- The collision detection is now faster and more robust.
- Code has been refactored such that we do not use STL containers anymore.
- The way to create a ConvexMeshShape has changed (see the user manual)
- The vertex indices stride has changed in the TriangleVertexArray for ConvexMeshShape (see the API documentation)
- The raycasting of a ConvexMeshShape does not use GJK algorithm anymore.
- The test do detect if a point is inside of a ConvexMeshShape does not use GJK algorithm anymore.
- A lot of optimizations have been performed and the library is now faster.
- Release code is now compiled with -O2 compiler optimization level (instead of none)
- Documentation has been updated
### Removed
- Quaternion constructor with Euler angles has been removed. The Quaternion::fromEulerAngles() method should be used instead.
- Cylinder and Cone collision shapes have been removed. The ConvexMeshShape collision shape should be used instead.
- The ProxyShape::setLocalScaling() method has been removed. The ConvexMeshShape, ConcaveMeshShape and HeightFieldShape
collision shapes can be scaled directly.
### Fixed
- Issues with checkboxes in testbed application were fixed.
- Fix issue with wrong AABB computation that can cause missing collision in broad-phase
- Bug [#26](https://github.com/DanielChappuis/reactphysics3d/issues/26) has been fixed.
- Bug [#30](https://github.com/DanielChappuis/reactphysics3d/issues/30) has been fixed.
- Bug [#32](https://github.com/DanielChappuis/reactphysics3d/issues/32) has been fixed.
- Issue [#31](https://github.com/DanielChappuis/reactphysics3d/issues/31) has been fixed.
- Issue [#34](https://github.com/DanielChappuis/reactphysics3d/issues/34) has been fixed.
- Issue [#37](https://github.com/DanielChappuis/reactphysics3d/issues/37) has been fixed.
## Version 0.6.0 (April 15, 2016)
### Added
- Support for static concave triangular mesh collision shape.
- Support for height field collision shape.
- Support for rolling resistance.
- It is now possible to change the local scaling of a collision shape.
- Add new constructor of ConvexMeshShape that accepts a triangular mesh.
- It is now easier to use your own narrow-phase collision detection algorithm.
- The CollisionWorld and DynamicsWorld now automatically destroys bodies and joints that have not been destroyed at the end.
- New testbed application with demo scenes.
### Changed
- The DynamicsWorld::update() method now takes the time step for the next simulation step in parameter.
## Version 0.5.0 (March 4, 2015)
### Added
- It is now possible to use multiple collision shapes per body.
- Ray casting support.
- Add methods to check if a point is inside a body or a proxy shape.
- Add collision filtering using collision categories (with bit masks).
- It is possible to attach user data to a body or a proxy shape.
- It is now possible to create a quaternion using Euler angles.
- It now possible to activate of deactivate a body.
- Differentiation between dynamic, kinematic and static bodies.
- Gravity can now be changed after the creation of the world.
- The center of mass and inertia tensor of a body can be set manually (center of mass can be different from body origin).
- Add a simulation step callback in the EventListener class that is called at each internal physics tick.
- Add methods to transform points/vectors from/into local-space/world-space coordinates of a body.
- Add CollisionWorld::testAABBOverlap() method to test overlap between two bodies or two proxy shapes.
- Add a ray casting example.
- Add unit tests for ray casting and collision detection.
### Changed
- Replace the Sweep-And-Prune algorithm by a Dynamic AABB Tree for the broad-phase collision detection.
- The center of mass of a body is now automatically computed from its collision shapes.
- Use GLFW instead of GLUT/Freeglut for the examples.
### Fixed
- Fix two issues in the EPA algorithm.
## Version 0.4.0 (October 7, 2013)
### Added
- Add collision shapes (Capsule, Convex Mesh).
- Add joints (Ball and Socket, Hinge, Slider, Fixed).
- Add sleeping technique for inactive bodies.
- Add velocity damping.
- It is now easier to apply force and torque to a rigid body.
- Add the EventListener class to allow the user to be notified when some events occur (contacts, …).
- Add examples for the joints and collision shapes.
- Make possible to modify the collision margin of some collision shapes.
- Add a Material class to keep track of the properties of a rigid body (friction coefficient, bounciness, …).
- Add a hierarchical real-time profiler.
### Changed
- Collision shapes now use the internal memory allocator.
- New internal memory allocator.
### Removed
- Remove the world gravity force from the external force of rigid bodies and allow the user to disable the gravity on a given body.
- Reduce the allocated memory of the broad-phase when several bodies are removed from the world.
### Fixed
- Fix issue in the Sweep-And-Prune broad-phase collision detection (thanks to Aleksi Sapon).
- Fix issue in the contact solver resulting in less jittering.
## Version 0.3.0
### Added
- Implementation of a dedicated collision detection algorithm for spheres against spheres instead of using GJK/EPA algorithm.
- Make possible to use a unique instance of a collision shape for multiple rigid bodies.
- Create the API documentation using Doxygen.
- Add Unit tests
### Changed
- The Sweep-and-Prune broad-phase collision detection algorithm has been rewritten according to the technique described by Pierre Terdiman at http://www.codercorner.com/SAP.pdf to be much more efficient than the previous naive implementation.
- The contact solver has been rewritten to use the Sequential Impulses technique from Erin Catto which is mathematically equivalent to the Projected Gauss Seidel technique that was used before. The Sequential Impulses technique is more intuitive.
- Make GJK/EPA algorithm more robust for spheres.
- Change the structure of the code for a better separation between the collision detection and the dynamics simulation code.
## Version 0.2.0
### Added
- Add the GJK/EPA algorithm for convex shapes collision detection
- Persistent contacts storing between frames
- Add Sphere, Cylinder and Cone collision shapes
- Add the Transform class for better handling of position and orientation of rigid bodies
- It is now possible to approximate the inertia tensor of rigid bodies using the collision shape inertia tensor.
- The AABB is now computed automatically using the collision shape
- Add the MemoryPool class to avoid intense dynamic allocation during the simulation
### Changed
- Simplification of the mathematics library
- Optimization of the constraint solver
- ReactPhysics3D is now under the zlib license

246
CMakeLists.txt
View File

@ -1,13 +1,16 @@
# Minimum cmake version required
CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3)
CMAKE_MINIMUM_REQUIRED(VERSION 3.2 FATAL_ERROR)
# Project configuration
PROJECT(REACTPHYSICS3D CXX)
PROJECT(REACTPHYSICS3D LANGUAGES CXX)
# In order to install libraries into correct locations on all platforms.
include(GNUInstallDirs)
# Build type
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "Release")
ENDIF (NOT CMAKE_BUILD_TYPE)
ENDIF()
# Where to build the library
SET(LIBRARY_OUTPUT_PATH "lib")
@ -15,18 +18,32 @@ SET(LIBRARY_OUTPUT_PATH "lib")
# Where to build the executables
SET(OUR_EXECUTABLE_OUTPUT_PATH "${PROJECT_BINARY_DIR}/bin")
# CMake modules path
SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules)
# Enable testing
ENABLE_TESTING()
# Options
OPTION(COMPILE_TESTBED "Select this if you want to build the testbed application" OFF)
OPTION(COMPILE_TESTS "Select this if you want to build the tests" OFF)
OPTION(PROFILING_ENABLED "Select this if you want to compile with enabled profiling" OFF)
OPTION(DOUBLE_PRECISION_ENABLED "Select this if you want to compile using double precision floating
OPTION(RP3D_COMPILE_TESTBED "Select this if you want to build the testbed application" OFF)
OPTION(RP3D_COMPILE_TESTS "Select this if you want to build the tests" OFF)
OPTION(RP3D_PROFILING_ENABLED "Select this if you want to compile with enabled profiling" OFF)
OPTION(RP3D_LOGS_ENABLED "Select this if you want to compile with logs enabled during execution" OFF)
OPTION(RP3D_CODE_COVERAGE_ENABLED "Select this if you need to build for code coverage calculation" OFF)
OPTION(RP3D_DOUBLE_PRECISION_ENABLED "Select this if you want to compile using double precision floating
values" OFF)
# Warning Compiler flags
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
IF(RP3D_CODE_COVERAGE_ENABLED)
IF(CMAKE_COMPILER_IS_GNUCXX)
INCLUDE(CodeCoverage)
SETUP_TARGET_FOR_COVERAGE(${PROJECT_NAME}_coverage tests coverage)
ENDIF()
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage")
ENDIF()
# C++11 flags
IF(CMAKE_CXX_COMPILER_ID MATCHES "Clang" OR CMAKE_CXX_COMPILER_ID MATCHES "GNU")
include(CheckCXXCompilerFlag)
@ -38,156 +55,207 @@ IF(CMAKE_CXX_COMPILER_ID MATCHES "Clang" OR CMAKE_CXX_COMPILER_ID MATCHES "GNU")
ENDIF()
ENDIF()
# Headers
INCLUDE_DIRECTORIES(src)
# Use libc++ with Clang
#IF(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
# SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++")
#ENDIF()
IF(PROFILING_ENABLED)
IF(RP3D_PROFILING_ENABLED)
ADD_DEFINITIONS(-DIS_PROFILING_ACTIVE)
ENDIF(PROFILING_ENABLED)
ENDIF()
IF(DOUBLE_PRECISION_ENABLED)
IF(RP3D_LOGS_ENABLED)
ADD_DEFINITIONS(-DIS_LOGGING_ACTIVE)
ENDIF()
IF(RP3D_DOUBLE_PRECISION_ENABLED)
ADD_DEFINITIONS(-DIS_DOUBLE_PRECISION_ENABLED)
ENDIF(DOUBLE_PRECISION_ENABLED)
ENDIF()
# Source files
SET (REACTPHYSICS3D_SOURCES
# Headers files
SET (REACTPHYSICS3D_HEADERS
"src/configuration.h"
"src/decimal.h"
"src/reactphysics3d.h"
"src/body/Body.h"
"src/body/Body.cpp"
"src/body/CollisionBody.h"
"src/body/CollisionBody.cpp"
"src/body/RigidBody.h"
"src/body/RigidBody.cpp"
"src/collision/ContactPointInfo.h"
"src/collision/ContactManifoldInfo.h"
"src/collision/broadphase/BroadPhaseAlgorithm.h"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"
"src/collision/broadphase/DynamicAABBTree.cpp"
"src/collision/narrowphase/CollisionDispatch.h"
"src/collision/narrowphase/DefaultCollisionDispatch.h"
"src/collision/narrowphase/DefaultCollisionDispatch.cpp"
"src/collision/narrowphase/EPA/EdgeEPA.h"
"src/collision/narrowphase/EPA/EdgeEPA.cpp"
"src/collision/narrowphase/EPA/EPAAlgorithm.h"
"src/collision/narrowphase/EPA/EPAAlgorithm.cpp"
"src/collision/narrowphase/EPA/TriangleEPA.h"
"src/collision/narrowphase/EPA/TriangleEPA.cpp"
"src/collision/narrowphase/EPA/TrianglesStore.h"
"src/collision/narrowphase/EPA/TrianglesStore.cpp"
"src/collision/narrowphase/GJK/Simplex.h"
"src/collision/narrowphase/GJK/Simplex.cpp"
"src/collision/narrowphase/GJK/VoronoiSimplex.h"
"src/collision/narrowphase/GJK/GJKAlgorithm.h"
"src/collision/narrowphase/GJK/GJKAlgorithm.cpp"
"src/collision/narrowphase/SAT/SATAlgorithm.h"
"src/collision/narrowphase/NarrowPhaseAlgorithm.h"
"src/collision/narrowphase/NarrowPhaseAlgorithm.cpp"
"src/collision/narrowphase/SphereVsSphereAlgorithm.h"
"src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h"
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.h"
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
"src/collision/shapes/AABB.h"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"
"src/collision/shapes/ConvexShape.cpp"
"src/collision/shapes/ConvexPolyhedronShape.h"
"src/collision/shapes/ConcaveShape.h"
"src/collision/shapes/ConcaveShape.cpp"
"src/collision/shapes/BoxShape.h"
"src/collision/shapes/BoxShape.cpp"
"src/collision/shapes/CapsuleShape.h"
"src/collision/shapes/CapsuleShape.cpp"
"src/collision/shapes/CollisionShape.h"
"src/collision/shapes/CollisionShape.cpp"
"src/collision/shapes/ConeShape.h"
"src/collision/shapes/ConeShape.cpp"
"src/collision/shapes/ConvexMeshShape.h"
"src/collision/shapes/ConvexMeshShape.cpp"
"src/collision/shapes/CylinderShape.h"
"src/collision/shapes/CylinderShape.cpp"
"src/collision/shapes/SphereShape.h"
"src/collision/shapes/SphereShape.cpp"
"src/collision/shapes/TriangleShape.h"
"src/collision/shapes/TriangleShape.cpp"
"src/collision/shapes/ConcaveMeshShape.h"
"src/collision/shapes/ConcaveMeshShape.cpp"
"src/collision/shapes/HeightFieldShape.h"
"src/collision/shapes/HeightFieldShape.cpp"
"src/collision/RaycastInfo.h"
"src/collision/RaycastInfo.cpp"
"src/collision/ProxyShape.h"
"src/collision/ProxyShape.cpp"
"src/collision/TriangleVertexArray.h"
"src/collision/TriangleVertexArray.cpp"
"src/collision/PolygonVertexArray.h"
"src/collision/TriangleMesh.h"
"src/collision/TriangleMesh.cpp"
"src/collision/PolyhedronMesh.h"
"src/collision/HalfEdgeStructure.h"
"src/collision/CollisionDetection.h"
"src/collision/CollisionDetection.cpp"
"src/collision/CollisionShapeInfo.h"
"src/collision/NarrowPhaseInfo.h"
"src/collision/ContactManifold.h"
"src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.h"
"src/collision/ContactManifoldSet.cpp"
"src/collision/MiddlePhaseTriangleCallback.h"
"src/constraint/BallAndSocketJoint.h"
"src/constraint/BallAndSocketJoint.cpp"
"src/constraint/ContactPoint.h"
"src/constraint/ContactPoint.cpp"
"src/constraint/FixedJoint.h"
"src/constraint/FixedJoint.cpp"
"src/constraint/HingeJoint.h"
"src/constraint/HingeJoint.cpp"
"src/constraint/Joint.h"
"src/constraint/Joint.cpp"
"src/constraint/SliderJoint.h"
"src/constraint/SliderJoint.cpp"
"src/engine/CollisionWorld.h"
"src/engine/CollisionWorld.cpp"
"src/engine/ConstraintSolver.h"
"src/engine/ConstraintSolver.cpp"
"src/engine/ContactSolver.h"
"src/engine/ContactSolver.cpp"
"src/engine/DynamicsWorld.h"
"src/engine/DynamicsWorld.cpp"
"src/engine/EventListener.h"
"src/engine/Impulse.h"
"src/engine/Island.h"
"src/engine/Island.cpp"
"src/engine/Material.h"
"src/engine/Material.cpp"
"src/engine/OverlappingPair.h"
"src/engine/OverlappingPair.cpp"
"src/engine/Profiler.h"
"src/engine/Profiler.cpp"
"src/engine/Timer.h"
"src/engine/Timer.cpp"
"src/collision/CollisionCallback.h"
"src/collision/OverlapCallback.h"
"src/mathematics/mathematics.h"
"src/mathematics/mathematics_functions.h"
"src/mathematics/mathematics_functions.cpp"
"src/mathematics/Matrix2x2.h"
"src/mathematics/Matrix2x2.cpp"
"src/mathematics/Matrix3x3.h"
"src/mathematics/Matrix3x3.cpp"
"src/mathematics/Quaternion.h"
"src/mathematics/Quaternion.cpp"
"src/mathematics/Transform.h"
"src/mathematics/Transform.cpp"
"src/mathematics/Vector2.h"
"src/mathematics/Vector2.cpp"
"src/mathematics/Vector3.h"
"src/mathematics/Ray.h"
"src/mathematics/Vector3.cpp"
"src/memory/MemoryAllocator.h"
"src/memory/MemoryAllocator.cpp"
"src/memory/Stack.h"
"src/memory/PoolAllocator.h"
"src/memory/SingleFrameAllocator.h"
"src/memory/DefaultAllocator.h"
"src/memory/MemoryManager.h"
"src/containers/Stack.h"
"src/containers/LinkedList.h"
"src/containers/List.h"
"src/containers/Map.h"
"src/containers/Set.h"
"src/containers/Pair.h"
"src/utils/Profiler.h"
"src/utils/Logger.h"
)
# Source files
SET (REACTPHYSICS3D_SOURCES
"src/body/Body.cpp"
"src/body/CollisionBody.cpp"
"src/body/RigidBody.cpp"
"src/collision/ContactManifoldInfo.cpp"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.cpp"
"src/collision/narrowphase/DefaultCollisionDispatch.cpp"
"src/collision/narrowphase/GJK/VoronoiSimplex.cpp"
"src/collision/narrowphase/GJK/GJKAlgorithm.cpp"
"src/collision/narrowphase/SAT/SATAlgorithm.cpp"
"src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp"
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp"
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.cpp"
"src/collision/shapes/ConvexPolyhedronShape.cpp"
"src/collision/shapes/ConcaveShape.cpp"
"src/collision/shapes/BoxShape.cpp"
"src/collision/shapes/CapsuleShape.cpp"
"src/collision/shapes/CollisionShape.cpp"
"src/collision/shapes/ConvexMeshShape.cpp"
"src/collision/shapes/SphereShape.cpp"
"src/collision/shapes/TriangleShape.cpp"
"src/collision/shapes/ConcaveMeshShape.cpp"
"src/collision/shapes/HeightFieldShape.cpp"
"src/collision/RaycastInfo.cpp"
"src/collision/ProxyShape.cpp"
"src/collision/TriangleVertexArray.cpp"
"src/collision/PolygonVertexArray.cpp"
"src/collision/TriangleMesh.cpp"
"src/collision/PolyhedronMesh.cpp"
"src/collision/HalfEdgeStructure.cpp"
"src/collision/CollisionDetection.cpp"
"src/collision/NarrowPhaseInfo.cpp"
"src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.cpp"
"src/collision/MiddlePhaseTriangleCallback.cpp"
"src/constraint/BallAndSocketJoint.cpp"
"src/constraint/ContactPoint.cpp"
"src/constraint/FixedJoint.cpp"
"src/constraint/HingeJoint.cpp"
"src/constraint/Joint.cpp"
"src/constraint/SliderJoint.cpp"
"src/engine/CollisionWorld.cpp"
"src/engine/ConstraintSolver.cpp"
"src/engine/ContactSolver.cpp"
"src/engine/DynamicsWorld.cpp"
"src/engine/Island.cpp"
"src/engine/Material.cpp"
"src/engine/OverlappingPair.cpp"
"src/engine/Timer.cpp"
"src/collision/CollisionCallback.cpp"
"src/mathematics/mathematics_functions.cpp"
"src/mathematics/Matrix2x2.cpp"
"src/mathematics/Matrix3x3.cpp"
"src/mathematics/Quaternion.cpp"
"src/mathematics/Transform.cpp"
"src/mathematics/Vector2.cpp"
"src/mathematics/Vector3.cpp"
"src/memory/PoolAllocator.cpp"
"src/memory/SingleFrameAllocator.cpp"
"src/memory/MemoryManager.cpp"
"src/utils/Profiler.cpp"
"src/utils/Logger.cpp"
)
# Create the library
ADD_LIBRARY(reactphysics3d STATIC ${REACTPHYSICS3D_SOURCES})
ADD_LIBRARY(reactphysics3d STATIC ${REACTPHYSICS3D_HEADERS} ${REACTPHYSICS3D_SOURCES})
# Headers
TARGET_INCLUDE_DIRECTORIES(reactphysics3d PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>
$<INSTALL_INTERFACE:include>
)
# If we need to compile the testbed application
IF(COMPILE_TESTBED)
IF(RP3D_COMPILE_TESTBED)
add_subdirectory(testbed/)
ENDIF(COMPILE_TESTBED)
ENDIF()
# If we need to compile the tests
IF(COMPILE_TESTS)
IF(RP3D_COMPILE_TESTS)
add_subdirectory(test/)
ENDIF(COMPILE_TESTS)
ENDIF()
# Install target
INSTALL(TARGETS reactphysics3d
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
)

197
CMakeModules/CodeCoverage.cmake Executable file
View File

@ -0,0 +1,197 @@
# Copyright (c) 2012 - 2015, Lars Bilke
# All rights reserved.
#
# 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 above copyright notice, this
# list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its contributors
# may be used to endorse or promote products derived from this software without
# specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.
#
#
#
# 2012-01-31, Lars Bilke
# - Enable Code Coverage
#
# 2013-09-17, Joakim Söderberg
# - Added support for Clang.
# - Some additional usage instructions.
#
# USAGE:
# 0. (Mac only) If you use Xcode 5.1 make sure to patch geninfo as described here:
# http://stackoverflow.com/a/22404544/80480
#
# 1. Copy this file into your cmake modules path.
#
# 2. Add the following line to your CMakeLists.txt:
# INCLUDE(CodeCoverage)
#
# 3. Set compiler flags to turn off optimization and enable coverage:
# SET(CMAKE_CXX_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage")
# SET(CMAKE_C_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage")
#
# 3. Use the function SETUP_TARGET_FOR_COVERAGE to create a custom make target
# which runs your test executable and produces a lcov code coverage report:
# Example:
# SETUP_TARGET_FOR_COVERAGE(
# my_coverage_target # Name for custom target.
# test_driver # Name of the test driver executable that runs the tests.
# # NOTE! This should always have a ZERO as exit code
# # otherwise the coverage generation will not complete.
# coverage # Name of output directory.
# )
#
# 4. Build a Debug build:
# cmake -DCMAKE_BUILD_TYPE=Debug ..
# make
# make my_coverage_target
#
#
# Check prereqs
FIND_PROGRAM( GCOV_PATH gcov )
FIND_PROGRAM( LCOV_PATH lcov )
FIND_PROGRAM( GENHTML_PATH genhtml )
FIND_PROGRAM( GCOVR_PATH gcovr PATHS ${CMAKE_SOURCE_DIR}/tests)
IF(NOT GCOV_PATH)
MESSAGE(FATAL_ERROR "gcov not found! Aborting...")
ENDIF() # NOT GCOV_PATH
IF("${CMAKE_CXX_COMPILER_ID}" MATCHES "(Apple)?[Cc]lang")
IF("${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS 3)
MESSAGE(FATAL_ERROR "Clang version must be 3.0.0 or greater! Aborting...")
ENDIF()
ELSEIF(NOT CMAKE_COMPILER_IS_GNUCXX)
MESSAGE(FATAL_ERROR "Compiler is not GNU gcc! Aborting...")
ENDIF() # CHECK VALID COMPILER
SET(CMAKE_CXX_FLAGS_COVERAGE
"-g -O0 --coverage -fprofile-arcs -ftest-coverage"
CACHE STRING "Flags used by the C++ compiler during coverage builds."
FORCE )
SET(CMAKE_C_FLAGS_COVERAGE
"-g -O0 --coverage -fprofile-arcs -ftest-coverage"
CACHE STRING "Flags used by the C compiler during coverage builds."
FORCE )
SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE
""
CACHE STRING "Flags used for linking binaries during coverage builds."
FORCE )
SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE
""
CACHE STRING "Flags used by the shared libraries linker during coverage builds."
FORCE )
MARK_AS_ADVANCED(
CMAKE_CXX_FLAGS_COVERAGE
CMAKE_C_FLAGS_COVERAGE
CMAKE_EXE_LINKER_FLAGS_COVERAGE
CMAKE_SHARED_LINKER_FLAGS_COVERAGE )
IF ( NOT (CMAKE_BUILD_TYPE STREQUAL "Debug" OR CMAKE_BUILD_TYPE STREQUAL "Coverage"))
MESSAGE( WARNING "Code coverage results with an optimized (non-Debug) build may be misleading" )
ENDIF() # NOT CMAKE_BUILD_TYPE STREQUAL "Debug"
# Param _targetname The name of new the custom make target
# Param _testrunner The name of the target which runs the tests.
# MUST return ZERO always, even on errors.
# If not, no coverage report will be created!
# Param _outputname lcov output is generated as _outputname.info
# HTML report is generated in _outputname/index.html
# Optional fourth parameter is passed as arguments to _testrunner
# Pass them in list form, e.g.: "-j;2" for -j 2
FUNCTION(SETUP_TARGET_FOR_COVERAGE _targetname _testrunner _outputname)
IF(NOT LCOV_PATH)
MESSAGE(FATAL_ERROR "lcov not found! Aborting...")
ENDIF() # NOT LCOV_PATH
IF(NOT GENHTML_PATH)
MESSAGE(FATAL_ERROR "genhtml not found! Aborting...")
ENDIF() # NOT GENHTML_PATH
SET(coverage_info "${CMAKE_BINARY_DIR}/${_outputname}.info")
SET(coverage_cleaned "${coverage_info}.cleaned")
SEPARATE_ARGUMENTS(test_command UNIX_COMMAND "${_testrunner}")
# Setup target
ADD_CUSTOM_TARGET(${_targetname}
# Cleanup lcov
${LCOV_PATH} --directory . --zerocounters
# Run tests
COMMAND ${test_command} ${ARGV3}
# Capturing lcov counters and generating report
COMMAND ${LCOV_PATH} --directory . --capture --output-file ${coverage_info}
COMMAND ${LCOV_PATH} --remove ${coverage_info} 'tests/*' '/usr/*' --output-file ${coverage_cleaned}
COMMAND ${GENHTML_PATH} -o ${_outputname} ${coverage_cleaned}
COMMAND ${CMAKE_COMMAND} -E remove ${coverage_info} ${coverage_cleaned}
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
COMMENT "Resetting code coverage counters to zero.\nProcessing code coverage counters and generating report."
)
# Show info where to find the report
ADD_CUSTOM_COMMAND(TARGET ${_targetname} POST_BUILD
COMMAND ;
COMMENT "Open ./${_outputname}/index.html in your browser to view the coverage report."
)
ENDFUNCTION() # SETUP_TARGET_FOR_COVERAGE
# Param _targetname The name of new the custom make target
# Param _testrunner The name of the target which runs the tests
# Param _outputname cobertura output is generated as _outputname.xml
# Optional fourth parameter is passed as arguments to _testrunner
# Pass them in list form, e.g.: "-j;2" for -j 2
FUNCTION(SETUP_TARGET_FOR_COVERAGE_COBERTURA _targetname _testrunner _outputname)
IF(NOT PYTHON_EXECUTABLE)
MESSAGE(FATAL_ERROR "Python not found! Aborting...")
ENDIF() # NOT PYTHON_EXECUTABLE
IF(NOT GCOVR_PATH)
MESSAGE(FATAL_ERROR "gcovr not found! Aborting...")
ENDIF() # NOT GCOVR_PATH
ADD_CUSTOM_TARGET(${_targetname}
# Run tests
${_testrunner} ${ARGV3}
# Running gcovr
COMMAND ${GCOVR_PATH} -x -r ${CMAKE_SOURCE_DIR} -e '${CMAKE_SOURCE_DIR}/tests/' -o ${_outputname}.xml
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
COMMENT "Running gcovr to produce Cobertura code coverage report."
)
# Show info where to find the report
ADD_CUSTOM_COMMAND(TARGET ${_targetname} POST_BUILD
COMMAND ;
COMMENT "Cobertura code coverage report saved in ${_outputname}.xml."
)
ENDFUNCTION() # SETUP_TARGET_FOR_COVERAGE_COBERTURA

51
GenerateNewVersion.py Normal file
View File

@ -0,0 +1,51 @@
# ----- Imports ----- #
import os
import fnmatch
import re
from datetime import date
# ----- Methods ----- #
# Method to find and replace some text in files inside a directory
def findReplaceText(directory, findRegex, substituteExpr, filePattern):
for path, dirs, files in os.walk(os.path.abspath(directory)):
for filename in fnmatch.filter(files, filePattern):
filepath = os.path.join(path, filename)
with open(filepath) as f:
s = f.read()
s = re.sub(findRegex, substituteExpr, s)
with open(filepath, "w") as f:
f.write(s)
# ----- Code ----- #
# Read new version number from user
oldVersion = raw_input("Enter the old version string: ")
# Read new version number from user
newVersion = raw_input("Enter the new version string: ")
print("ReactPhysics3D will be updated from version " + oldVersion + " to version " + newVersion)
# Replace the RP3D version number in the VERSION file
file = open("VERSION", "w")
file.write(newVersion + "\n")
file.close()
print("Version number has been updated in VERSION file")
# Update the RP3D version number in the documentation/API/Doxyfile
findReplaceText("documentation/API/", r'(PROJECT_NUMBER[ \t]+=[ \t]+)"[\d\.]+"', r'\g<1>"' + newVersion + '"', "Doxyfile")
print("Version number has been updated in documentation/API/Doxyfile file")
# Update the RP3D version number in the src/configuration.h file
findReplaceText("src/", r'(RP3D_VERSION[ \t]+=[ \t]+std::string\()"[\d\.]+"', r'\g<1>"' + newVersion + '"', "configuration.h")
print("Version number has been updated in src/configuration.h file")
# Update the copyright date in LICENSE file
findReplaceText("./", '(Copyright ' + re.escape("(c)") + r' 2010-)[\d]+', r'\g<1>' + str(date.today().year), "LICENSE")
print("Copyright date has been updated in LICENSE file")
# Update the copyright date of the license in every source code files
findReplaceText("src/", '(Copyright ' + re.escape("(c)") + r' 2010-)[\d]+', r'\g<1>' + str(date.today().year), "*.h")
findReplaceText("src/", '(Copyright ' + re.escape("(c)") + r' 2010-)[\d]+', r'\g<1>' + str(date.today().year), "*.cpp")
print("Copyright date in license has been updated in all source code files")

17
LICENSE
View File

@ -1,20 +1,17 @@
ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
Copyright (c) 2010-2016 Daniel Chappuis
Copyright (c) 2010-2018 Daniel Chappuis http://www.reactphysics3d.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the
use of this software.
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim
that you wrote the original software. If you use this software in a
product, an acknowledgment in the product documentation would be
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.

41
README.md
View File

@ -1,10 +1,12 @@
[![Travis Build Status](https://travis-ci.org/DanielChappuis/reactphysics3d.svg?branch=master)](https://travis-ci.org/DanielChappuis/reactphysics3d)
[![Codacy Badge](https://api.codacy.com/project/badge/Grade/3ae24e998e304e4da78ec848eade9e3a)](https://www.codacy.com/app/chappuis.daniel/reactphysics3d?utm_source=github.com&amp;utm_medium=referral&amp;utm_content=DanielChappuis/reactphysics3d&amp;utm_campaign=Badge_Grade)
[![codecov.io](https://codecov.io/github/DanielChappuis/reactphysics3d/coverage.svg?branch=master)](https://codecov.io/github/DanielChappuis/reactphysics3d?branch=master)
## ReactPhysics3D
ReactPhysics3D is an open source C++ physics engine library that can be used in 3D simulations and games.
Website : [http://www.reactphysics3d.com](http://www.reactphysics3d.com)
Website : [https://www.reactphysics3d.com](https://www.reactphysics3d.com)
Author : Daniel Chappuis
@ -14,23 +16,24 @@ Author : Daniel Chappuis
ReactPhysics3D has the following features :
- Rigid body dynamics
- Discrete collision detection
- Collision shapes (Sphere, Box, Cone, Cylinder, Capsule, Convex Mesh, static Concave Mesh, Height Field)
- Multiple collision shapes per body
- Broadphase collision detection (Dynamic AABB tree)
- Narrowphase collision detection (GJK/EPA)
- Collision response and friction (Sequential Impulses Solver)
- Joints (Ball and Socket, Hinge, Slider, Fixed)
- Collision filtering with categories
- Ray casting
- Sleeping technique for inactive bodies
- Integrated Profiler
- Multi-platform (Windows, Linux, Mac OS X)
- No dependencies (only OpenGL for the testbed application)
- Documentation (User manual and Doxygen API)
- Testbed application with demo scenes
- Unit tests
- Rigid body dynamics
- Discrete collision detection
- Collision shapes (Sphere, Box, Capsule, Convex Mesh, Static Concave Mesh, Height Field)
- Multiple collision shapes per body
- Broadphase collision detection (Dynamic AABB tree)
- Narrowphase collision detection (SAT/GJK)
- Collision response and friction (Sequential Impulses Solver)
- Joints (Ball and Socket, Hinge, Slider, Fixed)
- Collision filtering with categories
- Ray casting
- Sleeping technique for inactive bodies
- Multi-platform (Windows, Linux, Mac OS X)
- No external libraries (do not use STL containers)
- Documentation (user manual and Doxygen API)
- Testbed application with demos
- Integrated Profiler
- Logs
- Unit tests
## License
@ -38,7 +41,7 @@ The ReactPhysics3D library is released under the open-source [ZLib license](http
## Documentation
You can find the User Manual and the Doxygen API Documentation [here](http://www.reactphysics3d.com/documentation.html)
You can find the user manual and the Doxygen API documentation [here](https://www.reactphysics3d.com/documentation.html)
## Branches

2
VERSION
View File

@ -1 +1 @@
0.6.0
0.7.0

65
cmake/FindGLEW.cmake
View File

@ -1,65 +0,0 @@
#
# Try to find GLEW library and include path.
# Once done this will define
#
# GLEW_FOUND
# GLEW_INCLUDE_PATH
# GLEW_LIBRARY
#
IF (WIN32)
FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h
$ENV{PROGRAMFILES}/GLEW/include
${GLEW_ROOT_DIR}/include
DOC "The directory where GL/glew.h resides")
IF (NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
FIND_LIBRARY( GLEW_LIBRARY
NAMES glew64 glew64s
PATHS
$ENV{PROGRAMFILES}/GLEW/lib
${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin
${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib
DOC "The GLEW library (64-bit)"
)
ELSE(NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
FIND_LIBRARY( GLEW_LIBRARY
NAMES glew GLEW glew32 glew32s
PATHS
$ENV{PROGRAMFILES}/GLEW/lib
${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin
${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib
DOC "The GLEW library"
)
ENDIF(NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
ELSE (WIN32)
FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h
/usr/include
/usr/local/include
/sw/include
/opt/local/include
${GLEW_ROOT_DIR}/include
DOC "The directory where GL/glew.h resides")
FIND_LIBRARY( GLEW_LIBRARY
NAMES GLEW glew
PATHS
/usr/lib64
/usr/lib
/usr/local/lib64
/usr/local/lib
/sw/lib
/opt/local/lib
${GLEW_ROOT_DIR}/lib
DOC "The GLEW library")
ENDIF (WIN32)
SET(GLEW_FOUND "NO")
IF (GLEW_INCLUDE_PATH AND GLEW_LIBRARY)
SET(GLEW_LIBRARIES ${GLEW_LIBRARY})
SET(GLEW_FOUND "YES")
ENDIF (GLEW_INCLUDE_PATH AND GLEW_LIBRARY)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(GLEW DEFAULT_MSG GLEW_LIBRARY GLEW_INCLUDE_PATH)

2
documentation/API/Doxyfile
View File

@ -32,7 +32,7 @@ PROJECT_NAME = "ReactPhysics3D"
# This could be handy for archiving the generated documentation or
# if some version control system is used.
PROJECT_NUMBER = "0.6.0"
PROJECT_NUMBER = "0.7.0"
# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer

BIN
documentation/UserManual/ReactPhysics3D-UserManual.pdf
View File

Binary file not shown.

745
documentation/UserManual/ReactPhysics3D-UserManual.tex
View File

File diff suppressed because it is too large Load Diff

BIN
documentation/UserManual/images/coneshape.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 16 KiB

BIN
documentation/UserManual/images/cylindershape.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 7.2 KiB

BIN
documentation/UserManual/images/testbed.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 114 KiB

After

Width:  |  Height:  |  Size: 154 KiB

2
documentation/UserManual/title.tex
View File

@ -10,7 +10,7 @@
\vskip 1.3cm
{\Huge \@title\par}%
\vskip 0.3cm
{\Large Version: 0.6.0\par}%
{\Large Version: 0.7.0\par}%
\vskip 0.3cm
{\Large \@author\par}%
\vskip 2cm

57
src/body/Body.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -26,6 +26,7 @@
// Libraries
#include "Body.h"
#include "collision/shapes/CollisionShape.h"
#include "utils/Logger.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -36,11 +37,59 @@ using namespace reactphysics3d;
*/
Body::Body(bodyindex id)
: mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true),
mIsSleeping(false), mSleepTime(0), mUserData(NULL) {
mIsSleeping(false), mSleepTime(0), mUserData(nullptr) {
#ifdef IS_LOGGING_ACTIVE
mLogger = nullptr;
#endif
}
// Destructor
Body::~Body() {
// Set whether or not the body is active
/**
* @param isActive True if you want to activate the body
*/
void Body::setIsActive(bool isActive) {
mIsActive = isActive;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set isActive=" +
(mIsActive ? "true" : "false"));
}
// Set the variable to know whether or not the body is sleeping
void Body::setIsSleeping(bool isSleeping) {
if (isSleeping) {
mSleepTime = decimal(0.0);
}
else {
if (mIsSleeping) {
mSleepTime = decimal(0.0);
}
}
if (mIsSleeping != isSleeping) {
mIsSleeping = isSleeping;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set isSleeping=" +
(mIsSleeping ? "true" : "false"));
}
}
// Set whether or not the body is allowed to go to sleep
/**
* @param isAllowedToSleep True if the body is allowed to sleep
*/
void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
mIsAllowedToSleep = isAllowedToSleep;
if (!mIsAllowedToSleep) setIsSleeping(false);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set isAllowedToSleep=" +
(mIsAllowedToSleep ? "true" : "false"));
}

78
src/body/Body.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,13 +27,15 @@
#define REACTPHYSICS3D_BODY_H
// Libraries
#include <stdexcept>
#include <cassert>
#include "configuration.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
// Declarations
class Logger;
// TODO : Make this class abstract
// Class Body
/**
@ -75,13 +77,11 @@ class Body {
/// Pointer that can be used to attach user data to the body
void* mUserData;
// -------------------- Methods -------------------- //
#ifdef IS_LOGGING_ACTIVE
/// Private copy-constructor
Body(const Body& body);
/// Private assignment operator
Body& operator=(const Body& body);
/// Logger
Logger* mLogger;
#endif
public :
@ -90,11 +90,17 @@ class Body {
/// Constructor
Body(bodyindex id);
/// Deleted copy-constructor
Body(const Body& body) = delete;
/// Deleted assignment operator
Body& operator=(const Body& body) = delete;
/// Destructor
virtual ~Body();
virtual ~Body() = default;
/// Return the ID of the body
bodyindex getID() const;
bodyindex getId() const;
/// Return whether or not the body is allowed to sleep
bool isAllowedToSleep() const;
@ -120,6 +126,12 @@ class Body {
/// Attach user data to this body
void setUserData(void* userData);
#ifdef IS_LOGGING_ACTIVE
/// Set the logger
void setLogger(Logger* logger);
#endif
/// Smaller than operator
bool operator<(const Body& body2) const;
@ -139,9 +151,9 @@ class Body {
// Return the id of the body
/**
* @return The ID of the body
* @return The id of the body
*/
inline bodyindex Body::getID() const {
inline bodyindex Body::getId() const {
return mID;
}
@ -153,16 +165,6 @@ inline bool Body::isAllowedToSleep() const {
return mIsAllowedToSleep;
}
// Set whether or not the body is allowed to go to sleep
/**
* @param isAllowedToSleep True if the body is allowed to sleep
*/
inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
mIsAllowedToSleep = isAllowedToSleep;
if (!mIsAllowedToSleep) setIsSleeping(false);
}
// Return whether or not the body is sleeping
/**
* @return True if the body is currently sleeping and false otherwise
@ -179,29 +181,6 @@ inline bool Body::isActive() const {
return mIsActive;
}
// Set whether or not the body is active
/**
* @param isActive True if you want to activate the body
*/
inline void Body::setIsActive(bool isActive) {
mIsActive = isActive;
}
// Set the variable to know whether or not the body is sleeping
inline void Body::setIsSleeping(bool isSleeping) {
if (isSleeping) {
mSleepTime = decimal(0.0);
}
else {
if (mIsSleeping) {
mSleepTime = decimal(0.0);
}
}
mIsSleeping = isSleeping;
}
// Return a pointer to the user data attached to this body
/**
* @return A pointer to the user data you have attached to the body
@ -218,6 +197,15 @@ inline void Body::setUserData(void* userData) {
mUserData = userData;
}
#ifdef IS_LOGGING_ACTIVE
// Set the logger
inline void Body::setLogger(Logger* logger) {
mLogger = logger;
}
#endif
// Smaller than operator
inline bool Body::operator<(const Body& body2) const {
return (mID < body2.mID);

162
src/body/CollisionBody.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,6 +27,8 @@
#include "CollisionBody.h"
#include "engine/CollisionWorld.h"
#include "collision/ContactManifold.h"
#include "collision/RaycastInfo.h"
#include "utils/Logger.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -38,14 +40,18 @@ using namespace reactphysics3d;
* @param id ID of the body
*/
CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL),
mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) {
: Body(id), mType(BodyType::DYNAMIC), mTransform(transform), mProxyCollisionShapes(nullptr),
mNbCollisionShapes(0), mContactManifoldsList(nullptr), mWorld(world) {
#ifdef IS_PROFILING_ACTIVE
mProfiler = nullptr;
#endif
}
// Destructor
CollisionBody::~CollisionBody() {
assert(mContactManifoldsList == NULL);
assert(mContactManifoldsList == nullptr);
// Remove all the proxy collision shapes of the body
removeAllCollisionShapes();
@ -70,12 +76,26 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform) {
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, decimal(1));
transform, decimal(1), mWorld.mMemoryManager);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
proxyShape->setProfiler(mProfiler);
#endif
#ifdef IS_LOGGING_ACTIVE
// Set the logger
proxyShape->setLogger(mLogger);
#endif
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == NULL) {
if (mProxyCollisionShapes == nullptr) {
mProxyCollisionShapes = proxyShape;
}
else {
@ -92,6 +112,13 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
mNbCollisionShapes++;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " added to body");
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape,
"ProxyShape " + std::to_string(proxyShape->getBroadPhaseId()) + ": collisionShape=" +
proxyShape->getCollisionShape()->to_string());
// Return a pointer to the collision shape
return proxyShape;
}
@ -107,22 +134,25 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* current = mProxyCollisionShapes;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " removed from body");
// If the the first proxy shape is the one to remove
if (current == proxyShape) {
mProxyCollisionShapes = current->mNext;
if (mIsActive) {
if (mIsActive && proxyShape->getBroadPhaseId() != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
current->~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
// Look for the proxy shape that contains the collision shape in parameter
while(current->mNext != NULL) {
while(current->mNext != nullptr) {
// If we have found the collision shape to remove
if (current->mNext == proxyShape) {
@ -131,12 +161,12 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* elementToRemove = current->mNext;
current->mNext = elementToRemove->mNext;
if (mIsActive) {
if (mIsActive && proxyShape->getBroadPhaseId() != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
}
elementToRemove->~ProxyShape();
mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape));
mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, elementToRemove, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
@ -152,23 +182,23 @@ void CollisionBody::removeAllCollisionShapes() {
ProxyShape* current = mProxyCollisionShapes;
// Look for the proxy shape that contains the collision shape in parameter
while(current != NULL) {
while(current != nullptr) {
// Remove the proxy collision shape
ProxyShape* nextElement = current->mNext;
if (mIsActive) {
if (mIsActive && current->getBroadPhaseId() != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
current->~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape));
// Get the next element in the list
current = nextElement;
}
mProxyCollisionShapes = NULL;
mProxyCollisionShapes = nullptr;
}
// Reset the contact manifold lists
@ -176,23 +206,23 @@ void CollisionBody::resetContactManifoldsList() {
// Delete the linked list of contact manifolds of that body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != NULL) {
ContactManifoldListElement* nextElement = currentElement->next;
while (currentElement != nullptr) {
ContactManifoldListElement* nextElement = currentElement->getNext();
// Delete the current element
currentElement->~ContactManifoldListElement();
mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, currentElement, sizeof(ContactManifoldListElement));
currentElement = nextElement;
}
mContactManifoldsList = NULL;
mContactManifoldsList = nullptr;
}
// Update the broad-phase state for this body (because it has moved for instance)
void CollisionBody::updateBroadPhaseState() const {
// For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Update the proxy
updateProxyShapeInBroadPhase(shape);
@ -202,12 +232,15 @@ void CollisionBody::updateBroadPhaseState() const {
// Update the broad-phase state of a proxy collision shape of the body
void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const {
// Recompute the world-space AABB of the collision shape
AABB aabb;
proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
if (proxyShape->getBroadPhaseId() != -1) {
// Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert);
// Recompute the world-space AABB of the collision shape
AABB aabb;
proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert) ;
}
}
// Set whether or not the body is active
@ -225,7 +258,7 @@ void CollisionBody::setIsActive(bool isActive) {
if (isActive) {
// For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Compute the world-space AABB of the new collision shape
AABB aabb;
@ -238,15 +271,22 @@ void CollisionBody::setIsActive(bool isActive) {
else { // If we have to deactivate the body
// For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Remove the proxy shape from the collision detection
mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
if (shape->getBroadPhaseId() != -1) {
// Remove the proxy shape from the collision detection
mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
}
}
// Reset the contact manifold list of the body
resetContactManifoldsList();
}
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set isActive=" +
(mIsActive ? "true" : "false"));
}
// Ask the broad-phase to test again the collision shapes of the body for collision
@ -254,7 +294,7 @@ void CollisionBody::setIsActive(bool isActive) {
void CollisionBody::askForBroadPhaseCollisionCheck() const {
// For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape);
}
@ -271,9 +311,9 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
// Reset the mIsAlreadyInIsland variable of the contact manifolds for
// this body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != NULL) {
currentElement->contactManifold->mIsAlreadyInIsland = false;
currentElement = currentElement->next;
while (currentElement != nullptr) {
currentElement->getContactManifold()->mIsAlreadyInIsland = false;
currentElement = currentElement->getNext();
nbManifolds++;
}
@ -289,7 +329,7 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
// For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Test if the point is inside the collision shape
if (shape->testPointInside(worldPoint)) return true;
@ -315,7 +355,7 @@ bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
Ray rayTemp(ray);
// For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Test if the ray hits the collision shape
if (shape->raycast(rayTemp, raycastInfo)) {
@ -335,12 +375,12 @@ AABB CollisionBody::getAABB() const {
AABB bodyAABB;
if (mProxyCollisionShapes == NULL) return bodyAABB;
if (mProxyCollisionShapes == nullptr) return bodyAABB;
mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform());
// For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != nullptr; shape = shape->mNext) {
// Compute the world-space AABB of the collision shape
AABB aabb;
@ -352,3 +392,49 @@ AABB CollisionBody::getAABB() const {
return bodyAABB;
}
// Set the current position and orientation
/**
* @param transform The transformation of the body that transforms the local-space
* of the body into world-space
*/
void CollisionBody::setTransform(const Transform& transform) {
// Update the transform of the body
mTransform = transform;
// Update the broad-phase state of the body
updateBroadPhaseState();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set transform=" + mTransform.to_string());
}
// Set the type of the body
/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow:
/// STATIC : A static body has infinite mass, zero velocity but the position can be
/// changed manually. A static body does not collide with other static or kinematic bodies.
/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
/// position is computed by the physics engine. A kinematic body does not collide with
/// other static or kinematic bodies.
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
/// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies.
/**
* @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
*/
void CollisionBody::setType(BodyType type) {
mType = type;
if (mType == BodyType::STATIC) {
// Update the broad-phase state of the body
updateBroadPhaseState();
}
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set type=" +
(mType == BodyType::STATIC ? "Static" : (mType == BodyType::DYNAMIC ? "Dynamic" : "Kinematic")));
}

106
src/body/CollisionBody.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,23 +27,23 @@
#define REACTPHYSICS3D_COLLISION_BODY_H
// Libraries
#include <stdexcept>
#include <cassert>
#include "Body.h"
#include "mathematics/Transform.h"
#include "collision/shapes/AABB.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/RaycastInfo.h"
#include "memory/MemoryAllocator.h"
#include "mathematics/Transform.h"
#include "configuration.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
// Class declarations
// Declarations
struct ContactManifoldListElement;
class ProxyShape;
class CollisionWorld;
class CollisionShape;
struct RaycastInfo;
class PoolAllocator;
class Profiler;
/// Enumeration for the type of a body
/// STATIC : A static body has infinite mass, zero velocity but the position can be
@ -54,7 +54,7 @@ class CollisionWorld;
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
/// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies.
enum BodyType {STATIC, KINEMATIC, DYNAMIC};
enum class BodyType {STATIC, KINEMATIC, DYNAMIC};
// Class CollisionBody
/**
@ -85,14 +85,15 @@ class CollisionBody : public Body {
/// Reference to the world the body belongs to
CollisionWorld& mWorld;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Private copy-constructor
CollisionBody(const CollisionBody& body);
/// Private assignment operator
CollisionBody& operator=(const CollisionBody& body);
/// Reset the contact manifold lists
void resetContactManifoldsList();
@ -120,7 +121,13 @@ class CollisionBody : public Body {
CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id);
/// Destructor
virtual ~CollisionBody();
virtual ~CollisionBody() override;
/// Deleted copy-constructor
CollisionBody(const CollisionBody& body) = delete;
/// Deleted assignment operator
CollisionBody& operator=(const CollisionBody& body) = delete;
/// Return the type of the body
BodyType getType() const;
@ -129,7 +136,7 @@ class CollisionBody : public Body {
void setType(BodyType type);
/// Set whether or not the body is active
virtual void setIsActive(bool isActive);
virtual void setIsActive(bool isActive) override;
/// Return the current position and orientation
const Transform& getTransform() const;
@ -153,6 +160,9 @@ class CollisionBody : public Body {
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
/// Test if the collision body overlaps with a given AABB
bool testAABBOverlap(const AABB& worldAABB) const;
/// Compute and return the AABB of the body by merging all proxy shapes AABBs
AABB getAABB() const;
@ -174,6 +184,13 @@ class CollisionBody : public Body {
/// Return the body local-space coordinates of a vector given in the world-space coordinates
Vector3 getLocalVector(const Vector3& worldVector) const;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler);
#endif
// -------------------- Friendship -------------------- //
friend class CollisionWorld;
@ -192,29 +209,6 @@ inline BodyType CollisionBody::getType() const {
return mType;
}
// Set the type of the body
/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow:
/// STATIC : A static body has infinite mass, zero velocity but the position can be
/// changed manually. A static body does not collide with other static or kinematic bodies.
/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
/// position is computed by the physics engine. A kinematic body does not collide with
/// other static or kinematic bodies.
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
/// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies.
/**
* @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
*/
inline void CollisionBody::setType(BodyType type) {
mType = type;
if (mType == STATIC) {
// Update the broad-phase state of the body
updateBroadPhaseState();
}
}
// Return the current position and orientation
/**
* @return The current transformation of the body that transforms the local-space
@ -224,20 +218,6 @@ inline const Transform& CollisionBody::getTransform() const {
return mTransform;
}
// Set the current position and orientation
/**
* @param transform The transformation of the body that transforms the local-space
* of the body into world-space
*/
inline void CollisionBody::setTransform(const Transform& transform) {
// Update the transform of the body
mTransform = transform;
// Update the broad-phase state of the body
updateBroadPhaseState();
}
// Return the first element of the linked list of contact manifolds involving this body
/**
* @return A pointer to the first element of the linked-list with the contact
@ -301,6 +281,24 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
return mTransform.getOrientation().getInverse() * worldVector;
}
/// Test if the collision body overlaps with a given AABB
/**
* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap
* @return True if the given AABB overlaps with the AABB of the collision body
*/
inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const {
return worldAABB.testCollision(getAABB());
}
#endif
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void CollisionBody::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

309
src/body/RigidBody.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -28,6 +28,7 @@
#include "constraint/Joint.h"
#include "collision/shapes/CollisionShape.h"
#include "engine/DynamicsWorld.h"
#include "utils/Profiler.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -39,18 +40,21 @@ using namespace reactphysics3d;
* @param id The ID of the body
*/
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
: CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
mJointsList(NULL) {
mIsGravityEnabled(true), mMaterial(world.mConfig), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
// Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
}
// Destructor
RigidBody::~RigidBody() {
assert(mJointsList == NULL);
assert(mJointsList == nullptr);
}
// Set the type of the body
@ -76,7 +80,7 @@ void RigidBody::setType(BodyType type) {
recomputeMassInformation();
// If it is a static body
if (mType == STATIC) {
if (mType == BodyType::STATIC) {
// Reset the velocity to zero
mLinearVelocity.setToZero();
@ -84,19 +88,24 @@ void RigidBody::setType(BodyType type) {
}
// If it is a static or a kinematic body
if (mType == STATIC || mType == KINEMATIC) {
if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
// Reset the inverse mass and inverse inertia tensor to zero
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
}
else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
if (mIsInertiaTensorSetByUser) {
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
}
}
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Awake the body
setIsSleeping(false);
@ -113,28 +122,65 @@ void RigidBody::setType(BodyType type) {
}
// Set the local inertia tensor of the body (in local-space coordinates)
/// If the inertia tensor is set with this method, it will not be computed
/// using the collision shapes of the body.
/**
* @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space
* coordinates
*/
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
if (mType != DYNAMIC) return;
mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
mIsInertiaTensorSetByUser = true;
mInertiaTensorLocal = inertiaTensorLocal;
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
}
// Set the inverse local inertia tensor of the body (in local-space coordinates)
/// If the inverse inertia tensor is set with this method, it will not be computed
/// using the collision shapes of the body.
/**
* @param inverseInertiaTensorLocal The 3x3 inverse inertia tensor matrix of the body in local-space
* coordinates
*/
void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal) {
mUserInertiaTensorLocalInverse = inverseInertiaTensorLocal;
mIsInertiaTensorSetByUser = true;
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set inverseInertiaTensorLocal=" + inverseInertiaTensorLocal.to_string());
}
// Set the local center of mass of the body (in local-space coordinates)
/// If you set the center of mass with the method, it will not be computed
/// automatically using collision shapes.
/**
* @param centerOfMassLocal The center of mass of the body in local-space
* coordinates
*/
void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
if (mType != DYNAMIC) return;
if (mType != BodyType::DYNAMIC) return;
mIsCenterOfMassSetByUser = true;
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
mCenterOfMassLocal = centerOfMassLocal;
@ -144,6 +190,9 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set centerOfMassLocal=" + centerOfMassLocal.to_string());
}
// Set the mass of the rigid body
@ -152,7 +201,7 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
*/
void RigidBody::setMass(decimal mass) {
if (mType != DYNAMIC) return;
if (mType != BodyType::DYNAMIC) return;
mInitMass = mass;
@ -163,29 +212,34 @@ void RigidBody::setMass(decimal mass) {
mInitMass = decimal(1.0);
mMassInverse = decimal(1.0);
}
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set mass=" + std::to_string(mass));
}
// Remove a joint from the joints list
void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) {
void RigidBody::removeJointFromJointsList(MemoryManager& memoryManager, const Joint* joint) {
assert(joint != NULL);
assert(mJointsList != NULL);
assert(joint != nullptr);
assert(mJointsList != nullptr);
// Remove the joint from the linked list of the joints of the first body
if (mJointsList->joint == joint) { // If the first element is the one to remove
JointListElement* elementToRemove = mJointsList;
mJointsList = elementToRemove->next;
elementToRemove->~JointListElement();
memoryAllocator.release(elementToRemove, sizeof(JointListElement));
memoryManager.release(MemoryManager::AllocationType::Pool,
elementToRemove, sizeof(JointListElement));
}
else { // If the element to remove is not the first one in the list
JointListElement* currentElement = mJointsList;
while (currentElement->next != NULL) {
while (currentElement->next != nullptr) {
if (currentElement->next->joint == joint) {
JointListElement* elementToRemove = currentElement->next;
currentElement->next = elementToRemove->next;
elementToRemove->~JointListElement();
memoryAllocator.release(elementToRemove, sizeof(JointListElement));
memoryManager.release(MemoryManager::AllocationType::Pool,
elementToRemove, sizeof(JointListElement));
break;
}
currentElement = currentElement->next;
@ -213,15 +267,27 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform,
decimal mass) {
assert(mass > decimal(0.0));
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, mass);
transform, mass, mWorld.mMemoryManager);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
proxyShape->setProfiler(mProfiler);
#endif
#ifdef IS_LOGGING_ACTIVE
// Set the logger
proxyShape->setLogger(mLogger);
#endif
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == NULL) {
if (mProxyCollisionShapes == nullptr) {
mProxyCollisionShapes = proxyShape;
}
else {
@ -242,6 +308,13 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
// collision shape
recomputeMassInformation();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " added to body");
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape,
"ProxyShape " + std::to_string(proxyShape->getBroadPhaseId()) + ": collisionShape=" +
proxyShape->getCollisionShape()->to_string());
// Return a pointer to the proxy collision shape
return proxyShape;
}
@ -262,6 +335,55 @@ void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) {
recomputeMassInformation();
}
// Set the variable to know if the gravity is applied to this rigid body
/**
* @param isEnabled True if you want the gravity to be applied to this body
*/
void RigidBody::enableGravity(bool isEnabled) {
mIsGravityEnabled = isEnabled;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set isGravityEnabled=" +
(mIsGravityEnabled ? "true" : "false"));
}
// Set the linear damping factor. This is the ratio of the linear velocity
// that the body will lose every at seconds of simulation.
/**
* @param linearDamping The linear damping factor of this body
*/
void RigidBody::setLinearDamping(decimal linearDamping) {
assert(linearDamping >= decimal(0.0));
mLinearDamping = linearDamping;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set linearDamping=" + std::to_string(mLinearDamping));
}
// Set the angular damping factor. This is the ratio of the angular velocity
// that the body will lose at every seconds of simulation.
/**
* @param angularDamping The angular damping factor of this body
*/
void RigidBody::setAngularDamping(decimal angularDamping) {
assert(angularDamping >= decimal(0.0));
mAngularDamping = angularDamping;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set angularDamping=" + std::to_string(mAngularDamping));
}
// Set a new material for this rigid body
/**
* @param material The material you want to set to the body
*/
void RigidBody::setMaterial(const Material& material) {
mMaterial = material;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set Material" + mMaterial.to_string());
}
// Set the linear velocity of the rigid body.
/**
* @param linearVelocity Linear velocity vector of the body
@ -269,7 +391,7 @@ void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) {
void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
// If it is a static body, we do nothing
if (mType == STATIC) return;
if (mType == BodyType::STATIC) return;
// Update the linear velocity of the current body state
mLinearVelocity = linearVelocity;
@ -278,6 +400,9 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
if (mLinearVelocity.lengthSquare() > decimal(0.0)) {
setIsSleeping(false);
}
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set linearVelocity=" + mLinearVelocity.to_string());
}
// Set the angular velocity.
@ -287,7 +412,7 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
// If it is a static body, we do nothing
if (mType == STATIC) return;
if (mType == BodyType::STATIC) return;
// Set the angular velocity
mAngularVelocity = angularVelocity;
@ -296,6 +421,9 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
if (mAngularVelocity.lengthSquare() > decimal(0.0)) {
setIsSleeping(false);
}
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set angularVelocity=" + mAngularVelocity.to_string());
}
// Set the current position and orientation
@ -316,8 +444,14 @@ void RigidBody::setTransform(const Transform& transform) {
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Update the broad-phase state of the body
updateBroadPhaseState();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set transform=" + mTransform.to_string());
}
// Recompute the center of mass, total mass and inertia tensor of the body using all
@ -326,67 +460,82 @@ void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0);
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mCenterOfMassLocal.setToZero();
if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero();
Matrix3x3 inertiaTensorLocal;
inertiaTensorLocal.setToZero();
// If it is STATIC or KINEMATIC body
if (mType == STATIC || mType == KINEMATIC) {
// If it is a STATIC or a KINEMATIC body
if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
mCenterOfMassWorld = mTransform.getPosition();
return;
}
assert(mType == DYNAMIC);
assert(mType == BodyType::DYNAMIC);
// Compute the total mass of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
mInitMass += shape->getMass();
mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
}
}
if (mInitMass > decimal(0.0)) {
mMassInverse = decimal(1.0) / mInitMass;
}
else {
mInitMass = decimal(1.0);
mMassInverse = decimal(1.0);
mCenterOfMassWorld = mTransform.getPosition();
return;
}
// Compute the center of mass
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
mCenterOfMassLocal *= mMassInverse;
mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
// Compute the total mass and inertia tensor using all the collision shapes
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Get the inertia tensor of the collision shape in its local-space
Matrix3x3 inertiaTensor;
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
// Convert the collision shape inertia tensor into the local-space of the body
const Transform& shapeTransform = shape->getLocalToBodyTransform();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
// center into a inertia tensor w.r.t to the body origin.
Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
decimal offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0));
offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare);
offsetMatrix[0] += offset * (-offset.x);
offsetMatrix[1] += offset * (-offset.y);
offsetMatrix[2] += offset * (-offset.z);
offsetMatrix *= shape->getMass();
mInertiaTensorLocal += inertiaTensor + offsetMatrix;
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal *= mMassInverse;
}
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
if (!mIsInertiaTensorSetByUser) {
// Compute the inertia tensor using all the collision shapes
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Get the inertia tensor of the collision shape in its local-space
Matrix3x3 inertiaTensor;
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
// Convert the collision shape inertia tensor into the local-space of the body
const Transform& shapeTransform = shape->getLocalToBodyTransform();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
// center into a inertia tensor w.r.t to the body origin.
Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
decimal offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0));
offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare);
offsetMatrix[0] += offset * (-offset.x);
offsetMatrix[1] += offset * (-offset.y);
offsetMatrix[2] += offset * (-offset.z);
offsetMatrix *= shape->getMass();
inertiaTensorLocal += inertiaTensor + offsetMatrix;
}
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
}
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
@ -395,20 +544,38 @@ void RigidBody::recomputeMassInformation() {
// Update the broad-phase state for this body (because it has moved for instance)
void RigidBody::updateBroadPhaseState() const {
PROFILE("RigidBody::updateBroadPhaseState()");
RP3D_PROFILE("RigidBody::updateBroadPhaseState()", mProfiler);
DynamicsWorld& world = static_cast<DynamicsWorld&>(mWorld);
const Vector3 displacement = world.mTimeStep * mLinearVelocity;
// For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Recompute the world-space AABB of the collision shape
AABB aabb;
shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform());
shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
}
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
void RigidBody::setProfiler(Profiler* profiler) {
CollisionBody::setProfiler(profiler);
// Set the profiler for each proxy shape
ProxyShape* proxyShape = getProxyShapesList();
while (proxyShape != nullptr) {
proxyShape->setProfiler(profiler);
proxyShape = proxyShape->getNext();
}
}
#endif

162
src/body/RigidBody.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -31,7 +31,6 @@
#include "CollisionBody.h"
#include "engine/Material.h"
#include "mathematics/mathematics.h"
#include "memory/MemoryAllocator.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
@ -40,6 +39,7 @@ namespace reactphysics3d {
struct JointListElement;
class Joint;
class DynamicsWorld;
class MemoryManager;
// Class RigidBody
/**
@ -50,6 +50,11 @@ class DynamicsWorld;
*/
class RigidBody : public CollisionBody {
private :
/// Index of the body in arrays for contact/constraint solver
uint mArrayIndex;
protected :
// -------------------- Attributes -------------------- //
@ -76,13 +81,16 @@ class RigidBody : public CollisionBody {
/// Current external torque on the body
Vector3 mExternalTorque;
/// Local inertia tensor of the body (in local-space) with respect to the
/// center of mass of the body
Matrix3x3 mInertiaTensorLocal;
/// Inverse Local inertia tensor of the body (in local-space) set
/// by the user with respect to the center of mass of the body
Matrix3x3 mUserInertiaTensorLocalInverse;
/// Inverse of the inertia tensor of the body
Matrix3x3 mInertiaTensorLocalInverse;
/// Inverse of the world inertia tensor of the body
Matrix3x3 mInertiaTensorInverseWorld;
/// Inverse of the mass of the body
decimal mMassInverse;
@ -99,24 +107,27 @@ class RigidBody : public CollisionBody {
decimal mAngularDamping;
/// First element of the linked list of joints involving this body
JointListElement* mJointsList;
JointListElement* mJointsList;
/// True if the center of mass is set by the user
bool mIsCenterOfMassSetByUser;
/// True if the inertia tensor is set by the user
bool mIsInertiaTensorSetByUser;
// -------------------- Methods -------------------- //
/// Private copy-constructor
RigidBody(const RigidBody& body);
/// Private assignment operator
RigidBody& operator=(const RigidBody& body);
/// Remove a joint from the joints list
void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
void removeJointFromJointsList(reactphysics3d::MemoryManager& memoryManager, const Joint* joint);
/// Update the transform of the body after a change of the center of mass
void updateTransformWithCenterOfMass();
/// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const;
virtual void updateBroadPhaseState() const override;
/// Update the world inverse inertia tensor of the body
void updateInertiaTensorInverseWorld();
public :
@ -126,13 +137,19 @@ class RigidBody : public CollisionBody {
RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id);
/// Destructor
virtual ~RigidBody();
virtual ~RigidBody() override;
/// Deleted copy-constructor
RigidBody(const RigidBody& body) = delete;
/// Deleted assignment operator
RigidBody& operator=(const RigidBody& body) = delete;
/// Set the type of the body (static, kinematic or dynamic)
void setType(BodyType type);
/// Set the current position and orientation
virtual void setTransform(const Transform& transform);
virtual void setTransform(const Transform& transform) override;
/// Return the mass of the body
decimal getMass() const;
@ -150,26 +167,26 @@ class RigidBody : public CollisionBody {
void setAngularVelocity(const Vector3& angularVelocity);
/// Set the variable to know whether or not the body is sleeping
virtual void setIsSleeping(bool isSleeping);
/// Return the local inertia tensor of the body (in body coordinates)
const Matrix3x3& getInertiaTensorLocal() const;
virtual void setIsSleeping(bool isSleeping) override;
/// Set the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
/// Set the inverse local inertia tensor of the body (in body coordinates)
void setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal);
/// Get the inverse local inertia tensor of the body (in body coordinates)
const Matrix3x3& getInverseInertiaTensorLocal() const;
/// Return the inverse of the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorInverseWorld() const;
/// Set the local center of mass of the body (in local-space coordinates)
void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
/// Set the mass of the rigid body
void setMass(decimal mass);
/// Return the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorWorld() const;
/// Return the inverse of the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorInverseWorld() const;
/// Return true if the gravity needs to be applied to this rigid body
bool isGravityEnabled() const;
@ -215,12 +232,19 @@ class RigidBody : public CollisionBody {
decimal mass);
/// Remove a collision shape from the body
virtual void removeCollisionShape(const ProxyShape* proxyShape);
virtual void removeCollisionShape(const ProxyShape* proxyShape) override;
/// Recompute the center of mass, total mass and inertia tensor of the body using all
/// the collision shapes attached to the body.
void recomputeMassInformation();
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler) override;
#endif
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
@ -255,28 +279,9 @@ inline Vector3 RigidBody::getAngularVelocity() const {
return mAngularVelocity;
}
// Return the local inertia tensor of the body (in local-space coordinates)
/**
* @return The 3x3 inertia tensor matrix of the body (in local-space coordinates)
*/
inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
return mInertiaTensorLocal;
}
// Return the inertia tensor in world coordinates.
/// The inertia tensor I_w in world coordinates is computed
/// with the local inertia tensor I_b in body coordinates
/// by I_w = R * I_b * R^T
/// where R is the rotation matrix (and R^T its transpose) of
/// the current orientation quaternion of the body
/**
* @return The 3x3 inertia tensor matrix of the body in world-space coordinates
*/
inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
// Compute and return the inertia tensor in world coordinates
return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal *
mTransform.getOrientation().getMatrix().getTranspose();
// Get the inverse local inertia tensor of the body (in body coordinates)
inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
return mInertiaTensorLocalInverse;
}
// Return the inverse of the inertia tensor in world coordinates.
@ -291,12 +296,19 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
*/
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
// Compute and return the inertia tensor in world coordinates
return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse *
mTransform.getOrientation().getMatrix().getTranspose();
return mInertiaTensorInverseWorld;
}
// Update the world inverse inertia tensor of the body
/// The inertia tensor I_w in world coordinates is computed with the
/// local inverse inertia tensor I_b^-1 in body coordinates
/// by I_w = R * I_b^-1 * R^T
/// where R is the rotation matrix (and R^T its transpose) of the
/// current orientation quaternion of the body
inline void RigidBody::updateInertiaTensorInverseWorld() {
Matrix3x3 orientation = mTransform.getOrientation().getMatrix();
mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose();
}
// Return true if the gravity needs to be applied to this rigid body
@ -307,14 +319,6 @@ inline bool RigidBody::isGravityEnabled() const {
return mIsGravityEnabled;
}
// Set the variable to know if the gravity is applied to this rigid body
/**
* @param isEnabled True if you want the gravity to be applied to this body
*/
inline void RigidBody::enableGravity(bool isEnabled) {
mIsGravityEnabled = isEnabled;
}
// Return a reference to the material properties of the rigid body
/**
* @return A reference to the material of the body
@ -323,14 +327,6 @@ inline Material& RigidBody::getMaterial() {
return mMaterial;
}
// Set a new material for this rigid body
/**
* @param material The material you want to set to the body
*/
inline void RigidBody::setMaterial(const Material& material) {
mMaterial = material;
}
// Return the linear velocity damping factor
/**
* @return The linear damping factor of this body
@ -339,16 +335,6 @@ inline decimal RigidBody::getLinearDamping() const {
return mLinearDamping;
}
// Set the linear damping factor. This is the ratio of the linear velocity
// that the body will lose every at seconds of simulation.
/**
* @param linearDamping The linear damping factor of this body
*/
inline void RigidBody::setLinearDamping(decimal linearDamping) {
assert(linearDamping >= decimal(0.0));
mLinearDamping = linearDamping;
}
// Return the angular velocity damping factor
/**
* @return The angular damping factor of this body
@ -357,16 +343,6 @@ inline decimal RigidBody::getAngularDamping() const {
return mAngularDamping;
}
// Set the angular damping factor. This is the ratio of the angular velocity
// that the body will lose at every seconds of simulation.
/**
* @param angularDamping The angular damping factor of this body
*/
inline void RigidBody::setAngularDamping(decimal angularDamping) {
assert(angularDamping >= decimal(0.0));
mAngularDamping = angularDamping;
}
// Return the first element of the linked list of joints involving this body
/**
* @return The first element of the linked-list of all the joints involving this body
@ -407,7 +383,7 @@ inline void RigidBody::setIsSleeping(bool isSleeping) {
inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
// If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return;
if (mType != BodyType::DYNAMIC) return;
// Awake the body if it was sleeping
if (mIsSleeping) {
@ -432,7 +408,7 @@ inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
// If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return;
if (mType != BodyType::DYNAMIC) return;
// Awake the body if it was sleeping
if (mIsSleeping) {
@ -455,7 +431,7 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
inline void RigidBody::applyTorque(const Vector3& torque) {
// If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return;
if (mType != BodyType::DYNAMIC) return;
// Awake the body if it was sleeping
if (mIsSleeping) {

83
src/collision/CollisionCallback.cpp Normal file
View File

@ -0,0 +1,83 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "collision/CollisionCallback.h"
#include "engine/OverlappingPair.h"
#include "memory/MemoryAllocator.h"
#include "collision/ContactManifold.h"
#include "memory/MemoryManager.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager) :
contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()),
body2(pair->getShape2()->getBody()),
proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()),
mMemoryManager(memoryManager) {
assert(pair != nullptr);
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair
ContactManifold* contactManifold = manifoldSet.getContactManifolds();
assert(contactManifold != nullptr);
while (contactManifold != nullptr) {
assert(contactManifold->getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
ContactManifoldListElement* element = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
contactManifoldElements);
contactManifoldElements = element;
contactManifold = contactManifold->getNext();
}
}
// Destructor
CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() {
// Release memory allocator for the contact manifold list elements
ContactManifoldListElement* element = contactManifoldElements;
while (element != nullptr) {
ContactManifoldListElement* nextElement = element->getNext();
// Delete and release memory
element->~ContactManifoldListElement();
mMemoryManager.release(MemoryManager::AllocationType::Pool, element,
sizeof(ContactManifoldListElement));
element = nextElement;
}
}

94
src/collision/CollisionCallback.h Normal file
View File

@ -0,0 +1,94 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_COLLISION_CALLBACK_H
#define REACTPHYSICS3D_COLLISION_CALLBACK_H
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class OverlappingPair;
class ContactManifold;
struct ContactManifoldListElement;
class CollisionBody;
class ProxyShape;
class MemoryManager;
// Class CollisionCallback
/**
* This class can be used to register a callback for collision test queries.
* You should implement your own class inherited from this one and implement
* the notifyContact() method. This method will called each time a contact
* point is reported.
*/
class CollisionCallback {
public:
// Structure CollisionCallbackInfo
/**
* This structure represents the contact information between two collision
* shapes of two bodies
*/
struct CollisionCallbackInfo {
public:
/// Pointer to the first element of the linked-list that contains contact manifolds
ContactManifoldListElement* contactManifoldElements;
/// Pointer to the first body of the contact
CollisionBody* body1;
/// Pointer to the second body of the contact
CollisionBody* body2;
/// Pointer to the proxy shape of first body
const ProxyShape* proxyShape1;
/// Pointer to the proxy shape of second body
const ProxyShape* proxyShape2;
/// Memory manager
MemoryManager& mMemoryManager;
// Constructor
CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager);
// Destructor
~CollisionCallbackInfo();
};
/// Destructor
virtual ~CollisionCallback() = default;
/// This method will be called for each reported contact point
virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
};
}
#endif

1176
src/collision/CollisionDetection.cpp
View File

File diff suppressed because it is too large Load Diff

244
src/collision/CollisionDetection.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -29,43 +29,24 @@
// Libraries
#include "body/CollisionBody.h"
#include "broadphase/BroadPhaseAlgorithm.h"
#include "collision/shapes/CollisionShape.h"
#include "engine/OverlappingPair.h"
#include "engine/EventListener.h"
#include "narrowphase/DefaultCollisionDispatch.h"
#include "memory/MemoryAllocator.h"
#include "constraint/ContactPoint.h"
#include <vector>
#include <map>
#include <set>
#include <utility>
#include "collision/narrowphase/DefaultCollisionDispatch.h"
#include "containers/Map.h"
#include "containers/Set.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class BroadPhaseAlgorithm;
class CollisionWorld;
class CollisionCallback;
// Class TestCollisionBetweenShapesCallback
class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
private:
CollisionCallback* mCollisionCallback;
public:
// Constructor
TestCollisionBetweenShapesCallback(CollisionCallback* callback)
: mCollisionCallback(callback) {
}
// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo);
};
class OverlapCallback;
class RaycastCallback;
class ContactPoint;
class MemoryManager;
class EventListener;
class CollisionDispatch;
// Class CollisionDetection
/**
@ -74,12 +55,15 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
* collide and then we run a narrow-phase algorithm to compute the
* collision contacts between bodies.
*/
class CollisionDetection : public NarrowPhaseCallback {
class CollisionDetection {
private :
// -------------------- Attributes -------------------- //
/// Memory manager
MemoryManager& mMemoryManager;
/// Collision Detection Dispatch configuration
CollisionDispatch* mCollisionDispatch;
@ -89,42 +73,39 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Collision detection matrix (algorithms to use)
NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
/// Pointer to the physics world
CollisionWorld* mWorld;
/// Broad-phase overlapping pairs
std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
/// Pointer to the first narrow-phase info of the linked list
NarrowPhaseInfo* mNarrowPhaseInfoList;
/// Overlapping pairs in contact (during the current Narrow-phase collision detection)
std::map<overlappingpairid, OverlappingPair*> mContactOverlappingPairs;
/// Broad-phase overlapping pairs
Map<Pair<uint, uint>, OverlappingPair*> mOverlappingPairs;
/// Broad-phase algorithm
BroadPhaseAlgorithm mBroadPhaseAlgorithm;
/// Narrow-phase GJK algorithm
// TODO : Delete this
GJKAlgorithm mNarrowPhaseGJKAlgorithm;
/// Set of pair of bodies that cannot collide between each other
std::set<bodyindexpair> mNoCollisionPairs;
Set<bodyindexpair> mNoCollisionPairs;
/// True if some collision shapes have been added previously
bool mIsCollisionShapesAdded;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Private copy-constructor
CollisionDetection(const CollisionDetection& collisionDetection);
/// Private assignment operator
CollisionDetection& operator=(const CollisionDetection& collisionDetection);
/// Compute the broad-phase collision detection
void computeBroadPhase();
/// Compute the middle-phase collision detection
void computeMiddlePhase();
/// Compute the narrow-phase collision detection
void computeNarrowPhase();
@ -132,32 +113,54 @@ class CollisionDetection : public NarrowPhaseCallback {
/// involed in the corresponding contact.
void addContactManifoldToBody(OverlappingPair* pair);
/// Delete all the contact points in the currently overlapping pairs
void clearContactPoints();
/// Fill-in the collision detection matrix
void fillInCollisionMatrix();
/// Return the corresponding narrow-phase algorithm
NarrowPhaseAlgorithm* selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type,
const CollisionShapeType& shape2Type) const;
/// Add all the contact manifold of colliding pairs to their bodies
void addAllContactManifoldsToBodies();
/// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
NarrowPhaseInfo** firstNarrowPhaseInfo);
/// Compute the middle-phase collision detection between two proxy shapes
NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair);
/// Convert the potential contact into actual contacts
void processAllPotentialContacts();
/// Process the potential contact manifold of a pair to create actual contact manifold
void processPotentialContacts(OverlappingPair* pair);
/// Report contacts for all the colliding overlapping pairs
void reportAllContacts();
/// Process the potential contacts where one collion is a concave shape
void processSmoothMeshContacts(OverlappingPair* pair);
public :
// -------------------- Methods -------------------- //
/// Constructor
CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator);
CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager);
/// Destructor
~CollisionDetection();
~CollisionDetection() = default;
/// Deleted copy-constructor
CollisionDetection(const CollisionDetection& collisionDetection) = delete;
/// Deleted assignment operator
CollisionDetection& operator=(const CollisionDetection& collisionDetection) = delete;
/// Set the collision dispatch configuration
void setCollisionDispatch(CollisionDispatch* collisionDispatch);
/// Return the Narrow-phase collision detection algorithm to use between two types of shapes
NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type,
CollisionShapeType shape2Type) const;
/// Add a proxy collision shape to the collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
@ -180,35 +183,33 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Compute the collision detection
void computeCollisionDetection();
/// Compute the collision detection
void testCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2);
/// Report collision between two sets of shapes
void reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) ;
/// Ray casting method
void raycast(RaycastCallback* raycastCallback, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const;
/// Test if the AABBs of two bodies overlap
bool testAABBOverlap(const CollisionBody* body1,
const CollisionBody* body2) const;
/// Report all the bodies that overlap with the aabb in parameter
void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
/// Test if the AABBs of two proxy shapes overlap
bool testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const;
/// Return true if two bodies overlap
bool testOverlap(CollisionBody* body1, CollisionBody* body2);
/// Report all the bodies that overlap with the body in parameter
void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
/// Test and report collisions between two bodies
void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
/// Test and report collisions between a body and all the others bodies of the world
void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF);
/// Test and report collisions between all shapes of the world
void testCollision(CollisionCallback* callback);
/// Allow the broadphase to notify the collision detection about an overlapping pair.
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
/// Compute the narrow-phase collision detection
void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2);
/// Return a reference to the memory manager
MemoryManager& getMemoryManager() const;
/// Return a pointer to the world
CollisionWorld* getWorld();
@ -216,14 +217,15 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Return the world event listener
EventListener* getWorldEventListener();
/// Return a reference to the world memory allocator
MemoryAllocator& getWorldMemoryAllocator();
#ifdef IS_PROFILING_ACTIVE
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
/// Set the profiler
void setProfiler(Profiler* profiler);
/// Create a new contact
void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
#endif
/// Return the world-space AABB of a given proxy shape
const AABB getWorldAABB(const ProxyShape* proxyShape) const;
// -------------------- Friendship -------------------- //
@ -231,20 +233,20 @@ class CollisionDetection : public NarrowPhaseCallback {
friend class ConvexMeshShape;
};
// Return the Narrow-phase collision detection algorithm to use between two types of shapes
inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type,
CollisionShapeType shape2Type) const {
return mCollisionMatrix[shape1Type][shape2Type];
}
// Set the collision dispatch configuration
inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDispatch = collisionDispatch;
mCollisionDispatch->init(this, &mMemoryAllocator);
// Fill-in the collision matrix with the new algorithms to use
fillInCollisionMatrix();
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
mCollisionDispatch->setProfiler(mProfiler);
#endif
}
// Add a body to the collision detection
@ -260,20 +262,23 @@ inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
// Add a pair of bodies that cannot collide with each other
inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) {
mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2));
mNoCollisionPairs.add(OverlappingPair::computeBodiesIndexPair(body1, body2));
}
// Remove a pair of bodies that cannot collide with each other
inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) {
mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2));
mNoCollisionPairs.remove(OverlappingPair::computeBodiesIndexPair(body1, body2));
}
// Ask for a collision shape to be tested again during broad-phase.
/// We simply put the shape in the list of collision shape that have moved in the
/// previous frame so that it is tested for collision again in the broad-phase.
inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
if (shape->getBroadPhaseId() != -1) {
mBroadPhaseAlgorithm.addMovedCollisionShape(shape->getBroadPhaseId());
}
}
// Update a proxy collision shape (that has moved for instance)
@ -282,30 +287,23 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con
mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
}
// Ray casting method
inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const {
// Return the corresponding narrow-phase algorithm
inline NarrowPhaseAlgorithm* CollisionDetection::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type,
const CollisionShapeType& shape2Type) const {
PROFILE("CollisionDetection::raycast()");
uint shape1Index = static_cast<unsigned int>(shape1Type);
uint shape2Index = static_cast<unsigned int>(shape2Type);
RaycastTest rayCastTest(raycastCallback);
// Ask the broad-phase algorithm to call the testRaycastAgainstShape()
// callback method for each proxy shape hit by the ray in the broad-phase
mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
}
// Test if the AABBs of two proxy shapes overlap
inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const {
// If one of the shape's body is not active, we return no overlap
if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) {
return false;
// Swap the shape types if necessary
if (shape1Index > shape2Index) {
const uint tempIndex = shape1Index;
shape1Index = shape2Index;
shape2Index = tempIndex;
}
return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
assert(shape1Index <= shape2Index);
return mCollisionMatrix[shape1Index][shape2Index];
}
// Return a pointer to the world
@ -313,6 +311,22 @@ inline CollisionWorld* CollisionDetection::getWorld() {
return mWorld;
}
// Return a reference to the memory manager
inline MemoryManager& CollisionDetection::getMemoryManager() const {
return mMemoryManager;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void CollisionDetection::setProfiler(Profiler* profiler) {
mProfiler = profiler;
mBroadPhaseAlgorithm.setProfiler(profiler);
mCollisionDispatch->setProfiler(profiler);
}
#endif
}
#endif

383
src/collision/ContactManifold.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -24,240 +24,201 @@
********************************************************************************/
// Libraries
#include <iostream>
#include "ContactManifold.h"
#include "constraint/ContactPoint.h"
#include "collision/ContactManifoldInfo.h"
using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings)
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) {
mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false),
mWorldSettings(worldSettings) {
// For each contact point info in the manifold
const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
while(pointInfo != nullptr) {
// Add the new contact point
addContactPoint(pointInfo);
pointInfo = pointInfo->next;
}
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
assert(mNbContactPoints > 0);
}
// Destructor
ContactManifold::~ContactManifold() {
clear();
// Delete all the contact points
ContactPoint* contactPoint = mContactPoints;
while(contactPoint != nullptr) {
ContactPoint* nextContactPoint = contactPoint->getNext();
// Delete the contact point
contactPoint->~ContactPoint();
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
contactPoint = nextContactPoint;
}
}
// Add a contact point in the manifold
void ContactManifold::addContactPoint(ContactPoint* contact) {
// For contact already in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point
// already in the manifold.
decimal distance = (mContactPoints[i]->getWorldPointOnBody1() -
contact->getWorldPointOnBody1()).lengthSquare();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
// Delete the new contact
contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint));
assert(mNbContactPoints > 0);
return;
}
}
// If the contact manifold is full
if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove);
}
// Add the new contact point in the manifold
mContactPoints[mNbContactPoints] = contact;
mNbContactPoints++;
// Remove a contact point
void ContactManifold::removeContactPoint(ContactPoint* contactPoint) {
assert(mNbContactPoints > 0);
}
assert(mContactPoints != nullptr);
assert(contactPoint != nullptr);
// Remove a contact point from the manifold
void ContactManifold::removeContactPoint(uint index) {
assert(index < mNbContactPoints);
assert(mNbContactPoints > 0);
// Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free
mContactPoints[index]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
// If we don't remove the last index
if (index < mNbContactPoints - 1) {
mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
}
ContactPoint* previous = contactPoint->getPrevious();
ContactPoint* next = contactPoint->getNext();
mNbContactPoints--;
}
// Update the contact manifold
/// First the world space coordinates of the current contacts in the manifold are recomputed from
/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
/// the contacts with a too large distance between the contact points in the plane orthogonal to the
/// contact normal.
void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
if (mNbContactPoints == 0) return;
// Update the world coordinates and penetration depth of the contact points in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
mContactPoints[i]->setWorldPointOnBody1(transform1 *
mContactPoints[i]->getLocalPointOnBody1());
mContactPoints[i]->setWorldPointOnBody2(transform2 *
mContactPoints[i]->getLocalPointOnBody2());
mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() -
mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
}
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD;
// Remove the contact points that don't represent very well the contact manifold
for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int>(mNbContactPoints));
// Compute the distance between contact points in the normal direction
decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();
// If the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) {
removeContactPoint(i);
}
else {
// Compute the distance of the two contact points in the plane
// orthogonal to the contact normal
Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
mContactPoints[i]->getNormal() * distanceNormal;
Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance
// threshold, we remove the contact
if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
removeContactPoint(i);
}
}
}
}
// Return the index of the contact point with the larger penetration depth.
/// This corresponding contact will be kept in the cache. The method returns -1 is
/// the new contact is the deepest.
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
int indexMaxPenetrationDepth = -1;
decimal maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache
for (uint i=0; i<mNbContactPoints; i++) {
// If the current contact has a larger penetration depth
if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i;
}
}
// Return the index of largest penetration depth
return indexMaxPenetrationDepth;
}
// Return the index that will be removed.
/// The index of the contact point with the larger penetration
/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
/// the different area and we want to keep the contacts with the largest area. The new point is also
/// kept. In order to compute the area of a quadrilateral, we use the formula :
/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
/// when we compute this area, we do not calculate it exactly but we
/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
/// by Erwin Coumans (http://wwww.bulletphysics.org).
int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 1) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 2) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 3) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare();
}
// Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3);
}
// Return the index of maximum area
int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
if (area0 < area1) {
if (area1 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area1 < area3) return 3;
else return 1;
}
if (previous != nullptr) {
previous->setNext(next);
}
else {
if (area0 < area2) {
if (area2 < area3) return 3;
else return 2;
mContactPoints = next;
}
if (next != nullptr) {
next->setPrevious(previous);
}
// Delete the contact point
contactPoint->~ContactPoint();
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
mNbContactPoints--;
assert(mNbContactPoints >= 0);
}
// Return the largest depth of all the contact points
decimal ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f;
assert(mNbContactPoints > 0);
ContactPoint* contactPoint = mContactPoints;
while(contactPoint != nullptr){
decimal depth = contactPoint->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
else {
if (area0 < area3) return 3;
else return 0;
contactPoint = contactPoint->getNext();
}
return largestDepth;
}
// Add a contact point
void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
assert(contactPointInfo != nullptr);
// Create the new contact point
ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo, mWorldSettings);
// Add the new contact point into the manifold
contactPoint->setNext(mContactPoints);
contactPoint->setPrevious(nullptr);
if (mContactPoints != nullptr) {
mContactPoints->setPrevious(contactPoint);
}
mContactPoints = contactPoint;
mNbContactPoints++;
}
// Set to true to make the manifold obsolete
void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoints) {
mIsObsolete = isObsolete;
if (setContactPoints) {
ContactPoint* contactPoint = mContactPoints;
while (contactPoint != nullptr) {
contactPoint->setIsObsolete(isObsolete);
contactPoint = contactPoint->getNext();
}
}
}
// Clear the contact manifold
void ContactManifold::clear() {
for (uint i=0; i<mNbContactPoints; i++) {
// Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free
mContactPoints[i]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint));
// Clear the obsolete contact points
void ContactManifold::clearObsoleteContactPoints() {
assert(mContactPoints != nullptr);
// For each contact point of the manifold
ContactPoint* contactPoint = mContactPoints;
while (contactPoint != nullptr) {
ContactPoint* nextContactPoint = contactPoint->getNext();
// If the contact point is obsolete
if (contactPoint->getIsObsolete()) {
// Remove the contact point
removeContactPoint(contactPoint);
}
contactPoint = nextContactPoint;
}
mNbContactPoints = 0;
assert(mNbContactPoints > 0);
assert(mContactPoints != nullptr);
}
// Make sure we do not have too much contact points by keeping only the best
// contact points of the manifold (with largest penetration depth)
void ContactManifold::reduce() {
assert(mContactPoints != nullptr);
// Remove contact points while there is too much contact points
while (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
removeNonOptimalContactPoint();
}
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD && mNbContactPoints > 0);
assert(mContactPoints != nullptr);
}
// Remove a contact point that is not optimal (with a small penetration depth)
void ContactManifold::removeNonOptimalContactPoint() {
assert(mContactPoints != nullptr);
assert(mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD);
// Get the contact point with the minimum penetration depth among all points
ContactPoint* contactPoint = mContactPoints;
ContactPoint* minContactPoint = nullptr;
decimal minPenetrationDepth = DECIMAL_LARGEST;
while (contactPoint != nullptr) {
ContactPoint* nextContactPoint = contactPoint->getNext();
if (contactPoint->getPenetrationDepth() < minPenetrationDepth) {
minContactPoint = contactPoint;
minPenetrationDepth = contactPoint->getPenetrationDepth();
}
contactPoint = nextContactPoint;
}
assert(minContactPoint != nullptr);
// Remove the non optimal contact point
removeContactPoint(minContactPoint);
assert(mNbContactPoints > 0);
assert(mContactPoints != nullptr);
}

287
src/collision/ContactManifold.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,20 +27,18 @@
#define REACTPHYSICS3D_CONTACT_MANIFOLD_H
// Libraries
#include <vector>
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class declarations
class ContactManifold;
class ContactManifoldInfo;
struct ContactPointInfo;
class CollisionBody;
class ContactPoint;
class PoolAllocator;
// Structure ContactManifoldListElement
/**
@ -48,40 +46,48 @@ class ContactManifold;
*/
struct ContactManifoldListElement {
public:
private:
// -------------------- Attributes -------------------- //
/// Pointer to the actual contact manifold
ContactManifold* contactManifold;
/// Pointer to a contact manifold with contact points
ContactManifold* mContactManifold;
/// Next element of the list
ContactManifoldListElement* next;
ContactManifoldListElement* mNext;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifoldListElement(ContactManifold* initContactManifold,
ContactManifoldListElement* initNext)
:contactManifold(initContactManifold), next(initNext) {
ContactManifoldListElement(ContactManifold* contactManifold,
ContactManifoldListElement* next)
:mContactManifold(contactManifold), mNext(next) {
}
/// Return the contact manifold
ContactManifold* getContactManifold() {
return mContactManifold;
}
/// Return the next element in the linked-list
ContactManifoldListElement* getNext() {
return mNext;
}
};
// Class ContactManifold
/**
* This class represents the set of contact points between two bodies.
* This class represents a set of contact points between two bodies that
* all have a similar contact normal direction. Usually, there is a single
* contact manifold when two convex shapes are in contact. However, when
* a convex shape collides with a concave shape, there might be several
* contact manifolds with different normal directions.
* The contact manifold is implemented in a way to cache the contact
* points among the frames for better stability following the
* "Contact Generation" presentation of Erwin Coumans at GDC 2010
* conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
* Some code of this class is based on the implementation of the
* btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
* The contacts between two bodies are added one after the other in the cache.
* When the cache is full, we have to remove one point. The idea is to keep
* the point with the deepest penetration depth and also to keep the
* points producing the larger area (for a more stable contact manifold).
* The new added point is always kept.
* points among the frames for better stability (warm starting of the
* contact solver)
*/
class ContactManifold {
@ -96,13 +102,10 @@ class ContactManifold {
ProxyShape* mShape2;
/// Contact points in the manifold
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Normal direction Id (Unique Id representing the normal direction)
short int mNormalDirectionId;
ContactPoint* mContactPoints;
/// Number of contacts in the cache
uint mNbContactPoints;
int8 mNbContactPoints;
/// First friction vector of the contact manifold
Vector3 mFrictionVector1;
@ -128,40 +131,106 @@ class ContactManifold {
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
/// Pointer to the next contact manifold in the linked-list
ContactManifold* mNext;
/// Pointer to the previous contact manifold in linked-list
ContactManifold* mPrevious;
/// True if the contact manifold is obsolete
bool mIsObsolete;
/// World settings
const WorldSettings& mWorldSettings;
// -------------------- Methods -------------------- //
/// Private copy-constructor
ContactManifold(const ContactManifold& contactManifold);
/// Private assignment operator
ContactManifold& operator=(const ContactManifold& contactManifold);
/// Return the index of maximum area
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
/// Return the index of the contact with the larger penetration depth.
int getIndexOfDeepestPenetration(ContactPoint* newContact) const;
/// Return the index that will be removed.
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
/// Remove a contact point from the manifold
void removeContactPoint(uint index);
/// Return true if the contact manifold has already been added into an island
bool isAlreadyInIsland() const;
/// Set the pointer to the next element in the linked-list
void setNext(ContactManifold* nextManifold);
/// Return true if the manifold is obsolete
bool getIsObsolete() const;
/// Set to true to make the manifold obsolete
void setIsObsolete(bool isObselete, bool setContactPoints);
/// Clear the obsolete contact points
void clearObsoleteContactPoints();
/// Return the contact normal direction Id of the manifold
short getContactNormalId() const;
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
/// set the first friction vector at the center of the contact manifold
void setFrictionVector1(const Vector3& mFrictionVector1);
/// set the second friction vector at the center of the contact manifold
void setFrictionVector2(const Vector3& mFrictionVector2);
/// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1);
/// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2);
/// Add a contact point
void addContactPoint(const ContactPointInfo* contactPointInfo);
/// Make sure we do not have too much contact points by keeping only the best ones
void reduce();
/// Remove a contact point that is not optimal (with a small penetration depth)
void removeNonOptimalContactPoint();
/// Remove a contact point
void removeContactPoint(ContactPoint* contactPoint);
/// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Set the pointer to the previous element in the linked-list
void setPrevious(ContactManifold* previousManifold);
/// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const;
/// Return the second friction vector at the center of the contact manifold
const Vector3& getFrictionVector2() const;
/// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const;
/// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const;
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short int normalDirectionId);
ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings);
/// Destructor
~ContactManifold();
/// Deleted copy-constructor
ContactManifold(const ContactManifold& contactManifold) = delete;
/// Deleted assignment operator
ContactManifold& operator=(const ContactManifold& contactManifold) = delete;
/// Return a pointer to the first proxy shape of the contact
ProxyShape* getShape1() const;
@ -174,68 +243,25 @@ class ContactManifold {
/// Return a pointer to the second body of the contact manifold
CollisionBody* getBody2() const;
/// Return the normal direction Id
short int getNormalDirectionId() const;
/// Add a contact point to the manifold
void addContactPoint(ContactPoint* contact);
/// Update the contact manifold.
void update(const Transform& transform1, const Transform& transform2);
/// Clear the contact manifold
void clear();
/// Return the number of contact points in the manifold
uint getNbContactPoints() const;
int8 getNbContactPoints() const;
/// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const;
/// Return a pointer to the first contact point of the manifold
ContactPoint* getContactPoints() const;
/// set the first friction vector at the center of the contact manifold
void setFrictionVector1(const Vector3& mFrictionVector1);
/// Return a pointer to the previous element in the linked-list
ContactManifold* getPrevious() const;
/// Return the second friction vector at the center of the contact manifold
const Vector3& getFrictionVector2() const;
/// set the second friction vector at the center of the contact manifold
void setFrictionVector2(const Vector3& mFrictionVector2);
/// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const;
/// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1);
/// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const;
/// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2);
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
/// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Return a contact point of the manifold
ContactPoint* getContactPoint(uint index) const;
/// Return the normalized averaged normal vector
Vector3 getAverageContactNormal() const;
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
/// Return a pointer to the next element in the linked-list
ContactManifold* getNext() const;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class Island;
friend class CollisionBody;
friend class ContactManifoldSet;
friend class ContactSolver;
};
// Return a pointer to the first proxy shape of the contact
@ -258,13 +284,8 @@ inline CollisionBody* ContactManifold::getBody2() const {
return mShape2->getBody();
}
// Return the normal direction Id
inline short int ContactManifold::getNormalDirectionId() const {
return mNormalDirectionId;
}
// Return the number of contact points in the manifold
inline uint ContactManifold::getNbContactPoints() const {
inline int8 ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
}
@ -323,10 +344,9 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
mRollingResistanceImpulse = rollingResistanceImpulse;
}
// Return a contact point of the manifold
inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
assert(index < mNbContactPoints);
return mContactPoints[index];
// Return a pointer to the first contact point of the manifold
inline ContactPoint* ContactManifold::getContactPoints() const {
return mContactPoints;
}
// Return true if the contact manifold has already been added into an island
@ -334,31 +354,32 @@ inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
// Return the normalized averaged normal vector
inline Vector3 ContactManifold::getAverageContactNormal() const {
Vector3 averageNormal;
for (uint i=0; i<mNbContactPoints; i++) {
averageNormal += mContactPoints[i]->getNormal();
}
return averageNormal.getUnit();
// Return a pointer to the previous element in the linked-list
inline ContactManifold* ContactManifold::getPrevious() const {
return mPrevious;
}
// Return the largest depth of all the contact points
inline decimal ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f;
// Set the pointer to the previous element in the linked-list
inline void ContactManifold::setPrevious(ContactManifold* previousManifold) {
mPrevious = previousManifold;
}
for (uint i=0; i<mNbContactPoints; i++) {
decimal depth = mContactPoints[i]->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
}
// Return a pointer to the next element in the linked-list
inline ContactManifold* ContactManifold::getNext() const {
return mNext;
}
return largestDepth;
// Set the pointer to the next element in the linked-list
inline void ContactManifold::setNext(ContactManifold* nextManifold) {
mNext = nextManifold;
}
// Return true if the manifold is obsolete
inline bool ContactManifold::getIsObsolete() const {
return mIsObsolete;
}
}
#endif

305
src/collision/ContactManifoldInfo.cpp Normal file
View File

@ -0,0 +1,305 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "ContactManifoldInfo.h"
#include "collision/ContactPointInfo.h"
using namespace reactphysics3d;
// Constructor
ContactManifoldInfo::ContactManifoldInfo(MemoryAllocator& allocator)
: mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator) {
}
// Destructor
ContactManifoldInfo::~ContactManifoldInfo() {
// Remove all the contact points
reset();
}
// Add a new contact point into the manifold
void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo) {
assert(contactPointInfo->penetrationDepth > decimal(0.0));
// Add it into the linked list of contact points
contactPointInfo->next = mContactPointsList;
mContactPointsList = contactPointInfo;
mNbContactPoints++;
}
// Remove all the contact points
void ContactManifoldInfo::reset() {
// Delete all the contact points in the linked list
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Call the constructor
elementToDelete->~ContactPointInfo();
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
mContactPointsList = nullptr;
mNbContactPoints = 0;
}
// Return the largest penetration depth among its contact points
decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
ContactPointInfo* contactPoint = mContactPointsList;
assert(contactPoint != nullptr);
decimal maxDepth = decimal(0.0);
while (contactPoint != nullptr) {
if (contactPoint->penetrationDepth > maxDepth) {
maxDepth = contactPoint->penetrationDepth;
}
contactPoint = contactPoint->next;
}
return maxDepth;
}
// Reduce the number of contact points of the currently computed manifold
// This is based on the technique described by Dirk Gregorius in his
// "Contacts Creation" GDC presentation. This method will reduce the number of
// contact points to a maximum of 4 points (but it can be less).
void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
assert(mContactPointsList != nullptr);
// The following algorithm only works to reduce to a maximum of 4 contact points
assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
// If there are too many contact points in the manifold
if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
uint nbReducedPoints = 0;
ContactPointInfo* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD];
for (int i=0; i<MAX_CONTACT_POINTS_IN_MANIFOLD; i++) {
pointsToKeep[i] = nullptr;
}
// Compute the initial contact point we need to keep.
// The first point we keep is always the point in a given
// constant direction (in order to always have same contact points
// between frames for better stability)
const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse();
// Compute the contact normal of the manifold (we use the first contact point)
// in the local-space of the first collision shape
const Vector3 contactNormalShape1Space = worldToShape1Transform.getOrientation() * mContactPointsList->normal;
// Compute a search direction
const Vector3 searchDirection(1, 1, 1);
ContactPointInfo* element = mContactPointsList;
pointsToKeep[0] = element;
decimal maxDotProduct = searchDirection.dot(element->localPoint1);
element = element->next;
nbReducedPoints = 1;
while(element != nullptr) {
decimal dotProduct = searchDirection.dot(element->localPoint1);
if (dotProduct > maxDotProduct) {
maxDotProduct = dotProduct;
pointsToKeep[0] = element;
}
element = element->next;
}
assert(pointsToKeep[0] != nullptr);
assert(nbReducedPoints == 1);
// Compute the second contact point we need to keep.
// The second point we keep is the one farthest away from the first point.
decimal maxDistance = decimal(0.0);
element = mContactPointsList;
while(element != nullptr) {
if (element == pointsToKeep[0]) {
element = element->next;
continue;
}
decimal distance = (pointsToKeep[0]->localPoint1 - element->localPoint1).lengthSquare();
if (distance >= maxDistance) {
maxDistance = distance;
pointsToKeep[1] = element;
nbReducedPoints = 2;
}
element = element->next;
}
assert(pointsToKeep[1] != nullptr);
assert(nbReducedPoints == 2);
// Compute the third contact point we need to keep.
// The second point is the one producing the triangle with the larger area
// with first and second point.
// We compute the most positive or most negative triangle area (depending on winding)
ContactPointInfo* thirdPointMaxArea = nullptr;
ContactPointInfo* thirdPointMinArea = nullptr;
decimal minArea = decimal(0.0);
decimal maxArea = decimal(0.0);
bool isPreviousAreaPositive = true;
element = mContactPointsList;
while(element != nullptr) {
if (element == pointsToKeep[0] || element == pointsToKeep[1]) {
element = element->next;
continue;
}
const Vector3 newToFirst = pointsToKeep[0]->localPoint1 - element->localPoint1;
const Vector3 newToSecond = pointsToKeep[1]->localPoint1 - element->localPoint1;
// Compute the triangle area
decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
if (area >= maxArea) {
maxArea = area;
thirdPointMaxArea = element;
}
if (area <= minArea) {
minArea = area;
thirdPointMinArea = element;
}
element = element->next;
}
assert(minArea <= decimal(0.0));
assert(maxArea >= decimal(0.0));
if (maxArea > (-minArea)) {
isPreviousAreaPositive = true;
pointsToKeep[2] = thirdPointMaxArea;
nbReducedPoints = 3;
}
else {
isPreviousAreaPositive = false;
pointsToKeep[2] = thirdPointMinArea;
nbReducedPoints = 3;
}
// Compute the 4th point by choosing the triangle that add the most
// triangle area to the previous triangle and has opposite sign area (opposite winding)
decimal largestArea = decimal(0.0); // Largest area (positive or negative)
element = mContactPointsList;
if (nbReducedPoints == 3) {
// For each remaining point
while(element != nullptr) {
if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) {
element = element->next;
continue;
}
// For each edge of the triangle made by the first three points
for (uint i=0; i<3; i++) {
uint edgeVertex1Index = i;
uint edgeVertex2Index = i < 2 ? i + 1 : 0;
const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->localPoint1 - element->localPoint1;
const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->localPoint1 - element->localPoint1;
// Compute the triangle area
decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
// We are looking at the triangle with maximal area (positive or negative).
// If the previous area is positive, we are looking at negative area now.
// If the previous area is negative, we are looking at the positive area now.
if (isPreviousAreaPositive && area <= largestArea) {
largestArea = area;
pointsToKeep[3] = element;
nbReducedPoints = 4;
}
else if (!isPreviousAreaPositive && area >= largestArea) {
largestArea = area;
pointsToKeep[3] = element;
nbReducedPoints = 4;
}
}
element = element->next;
}
}
// Delete the contact points we do not want to keep from the linked list
element = mContactPointsList;
ContactPointInfo* previousElement = nullptr;
while(element != nullptr) {
bool deletePoint = true;
// Skip the points we want to keep
for (uint i=0; i<nbReducedPoints; i++) {
if (element == pointsToKeep[i]) {
previousElement = element;
element = element->next;
deletePoint = false;
}
}
if (deletePoint) {
ContactPointInfo* elementToDelete = element;
if (previousElement != nullptr) {
previousElement->next = elementToDelete->next;
}
else {
mContactPointsList = elementToDelete->next;
}
element = element->next;
// Call the destructor
elementToDelete->~ContactPointInfo();
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
}
assert(nbReducedPoints > 0 && nbReducedPoints <= 4);
mNbContactPoints = nbReducedPoints;
}
}

117
src/collision/ContactManifoldInfo.h Normal file
View File

@ -0,0 +1,117 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
// Libraries
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class MemoryAllocator;
struct ContactPointInfo;
class Transform;
// Constants
const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class ContactManifoldInfo
/**
* This class is used to collect the list of ContactPointInfo that come
* from a collision test between two shapes.
*/
class ContactManifoldInfo {
private:
// -------------------- Attributes -------------------- //
/// Linked list with all the contact points
ContactPointInfo* mContactPointsList;
/// Number of contact points in the manifold
uint mNbContactPoints;
/// Next element in the linked-list of contact manifold info
ContactManifoldInfo* mNext;
/// Reference the the memory allocator where the contact point infos have been allocated
MemoryAllocator& mAllocator;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifoldInfo(MemoryAllocator& allocator);
/// Destructor
~ContactManifoldInfo();
/// Deleted Copy-constructor
ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
/// Deleted assignment operator
ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete;
/// Add a new contact point into the manifold
void addContactPoint(ContactPointInfo* contactPointInfo);
/// Remove all the contact points
void reset();
/// Get the first contact point info of the linked list of contact points
ContactPointInfo* getFirstContactPointInfo() const;
/// Return the largest penetration depth among its contact points
decimal getLargestPenetrationDepth() const;
/// Return the pointer to the next manifold info in the linked-list
ContactManifoldInfo* getNext();
/// Reduce the number of contact points of the currently computed manifold
void reduce(const Transform& shape1ToWorldTransform);
// Friendship
friend class OverlappingPair;
friend class CollisionDetection;
};
// Get the first contact point info of the linked list of contact points
inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const {
return mContactPointsList;
}
// Return the pointer to the next manifold info in the linked-list
inline ContactManifoldInfo* ContactManifoldInfo::getNext() {
return mNext;
}
}
#endif

397
src/collision/ContactManifoldSet.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -25,15 +25,21 @@
// Libraries
#include "ContactManifoldSet.h"
#include "constraint/ContactPoint.h"
#include "collision/ContactManifoldInfo.h"
#include "ProxyShape.h"
#include "collision/ContactManifold.h"
using namespace reactphysics3d;
// Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds)
: mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator) {
assert(nbMaxManifolds >= 1);
MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings)
: mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr), mWorldSettings(worldSettings) {
// Compute the maximum number of manifolds allowed between the two shapes
mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape());
}
// Destructor
@ -43,194 +49,267 @@ ContactManifoldSet::~ContactManifoldSet() {
clear();
}
// Add a contact point to the manifold set
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) {
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contact->getNormal());
assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr);
// If there is no contact manifold yet
if (mNbManifolds == 0) {
createManifold(normalDirectionId);
mManifolds[0]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
}
return;
}
// Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = 0;
if (mNbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
}
// Try to find an existing contact manifold with similar contact normal
ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo);
// If a similar manifold has been found
if (similarManifoldIndex != -1) {
if (similarManifold != nullptr) {
// Add the contact point to that similar manifold
mManifolds[similarManifoldIndex]->addContactPoint(contact);
assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0);
return;
}
// If the maximum number of manifold has not been reached yet
if (mNbManifolds < mNbMaxManifolds) {
// Create a new manifold for the contact point
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
}
return;
}
// The contact point will be in a new contact manifold, we now have too much
// manifolds condidates. We need to remove one. We choose to keep the manifolds
// with the largest contact depth among their points
int smallestDepthIndex = -1;
decimal minDepth = contact->getPenetrationDepth();
assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
smallestDepthIndex = i;
}
}
// If we do not want to keep to new manifold (not created yet) with the
// new contact point
if (smallestDepthIndex == -1) {
// Delete the new contact
contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint));
return;
}
assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds);
// Here we need to replace an existing manifold with a new one (that contains
// the new contact point)
removeManifold(smallestDepthIndex);
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
}
return;
}
// Return the index of the contact manifold with a similar average normal.
// If no manifold has close enough average normal, it returns -1
int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
// Return the Id of the manifold with the same normal direction id (if exists)
for (int i=0; i<mNbManifolds; i++) {
if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
return i;
}
}
return -1;
}
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
assert(normal.lengthSquare() > MACHINE_EPSILON);
int faceNo;
decimal u, v;
decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
Vector3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
faceNo = normalScaled.x > 0 ? 0 : 1;
u = normalScaled.y;
v = normalScaled.z;
}
else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
faceNo = normalScaled.y > 0 ? 2 : 3;
u = normalScaled.x;
v = normalScaled.z;
// Update the old manifold with the new one
updateManifoldWithNewOne(similarManifold, contactManifoldInfo);
}
else {
faceNo = normalScaled.z > 0 ? 4 : 5;
u = normalScaled.x;
v = normalScaled.y;
// Create a new contact manifold
createManifold(contactManifoldInfo);
}
int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
return faceNo * 200 + indexU * nbSubDivInFace + indexV;
}
// Update the contact manifolds
void ContactManifoldSet::update() {
// Return the total number of contact points in the set of manifolds
int ContactManifoldSet::getTotalNbContactPoints() const {
int nbPoints = 0;
for (int i=mNbManifolds-1; i>=0; i--) {
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
nbPoints += manifold->getNbContactPoints();
// Update the contact manifold
mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform());
// Remove the contact manifold if has no contact points anymore
if (mManifolds[i]->getNbContactPoints() == 0) {
removeManifold(i);
}
manifold = manifold->getNext();
}
return nbPoints;
}
// Return the maximum number of contact manifolds allowed between to collision shapes
int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) {
// If both shapes are convex
if (shape1->isConvex() && shape2->isConvex()) {
return mWorldSettings.nbMaxContactManifoldsConvexShape;
} // If there is at least one concave shape
else {
return mWorldSettings.nbMaxContactManifoldsConcaveShape;
}
}
// Update a previous similar manifold with a new one
void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
assert(oldManifold != nullptr);
assert(newManifold != nullptr);
// For each contact point of the new manifold
ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
assert(contactPointInfo != nullptr);
while (contactPointInfo != nullptr) {
// For each contact point in the old manifold
bool isSimilarPointFound = false;
ContactPoint* oldContactPoint = oldManifold->getContactPoints();
while (oldContactPoint != nullptr) {
assert(oldContactPoint != nullptr);
// If the new contact point is similar (very close) to the old contact point
if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
// Replace (update) the old contact point with the new one
oldContactPoint->update(contactPointInfo);
isSimilarPointFound = true;
break;
}
oldContactPoint = oldContactPoint->getNext();
}
// If we have not found a similar contact point
if (!isSimilarPointFound) {
// Add the contact point to the manifold
oldManifold->addContactPoint(contactPointInfo);
}
contactPointInfo = contactPointInfo->next;
}
// The old manifold is no longer obsolete
oldManifold->setIsObsolete(false, false);
}
// Remove a contact manifold that is the least optimal (smaller penetration depth)
void ContactManifoldSet::removeNonOptimalManifold() {
assert(mNbManifolds > mNbMaxManifolds);
assert(mManifolds != nullptr);
// Look for a manifold that is not new and with the smallest contact penetration depth.
// At the same time, we also look for a new manifold with the smallest contact penetration depth
// in case no old manifold exists.
ContactManifold* minDepthManifold = nullptr;
decimal minDepth = DECIMAL_LARGEST;
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
// Get the largest contact point penetration depth of the manifold
const decimal depth = manifold->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
minDepthManifold = manifold;
}
manifold = manifold->getNext();
}
// Remove the non optimal manifold
assert(minDepthManifold != nullptr);
removeManifold(minDepthManifold);
}
// Return the contact manifold with a similar contact normal.
// If no manifold has close enough contact normal, it returns nullptr
ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const {
// Get the contact normal of the first point of the manifold
const ContactPointInfo* contactPoint = contactManifold->getFirstContactPointInfo();
assert(contactPoint != nullptr);
ContactManifold* manifold = mManifolds;
// Return the Id of the manifold with the same normal direction id (if exists)
while (manifold != nullptr) {
// Get the first contact point of the current manifold
const ContactPoint* point = manifold->getContactPoints();
assert(point != nullptr);
// If the contact normal of the two manifolds are close enough
if (contactPoint->normal.dot(point->getNormal()) >= mWorldSettings.cosAngleSimilarContactManifold) {
return manifold;
}
manifold = manifold->getNext();
}
return nullptr;
}
// Clear the contact manifold set
void ContactManifoldSet::clear() {
// Destroy all the contact manifolds
for (int i=mNbManifolds-1; i>=0; i--) {
removeManifold(i);
ContactManifold* manifold = mManifolds;
while(manifold != nullptr) {
ContactManifold* nextManifold = manifold->getNext();
// Delete the contact manifold
manifold->~ContactManifold();
mMemoryAllocator.release(manifold, sizeof(ContactManifold));
manifold = nextManifold;
mNbManifolds--;
}
assert(mNbManifolds == 0);
}
// Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(short int normalDirectionId) {
assert(mNbManifolds < mNbMaxManifolds);
void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, mWorldSettings);
manifold->setPrevious(nullptr);
manifold->setNext(mManifolds);
if (mManifolds != nullptr) {
mManifolds->setPrevious(manifold);
}
mManifolds = manifold;
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId);
mNbManifolds++;
}
// Remove a contact manifold from the set
void ContactManifoldSet::removeManifold(int index) {
void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
assert(mNbManifolds > 0);
assert(index >= 0 && index < mNbManifolds);
assert(manifold != nullptr);
// Delete the new contact
mManifolds[index]->~ContactManifold();
mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
ContactManifold* previous = manifold->getPrevious();
ContactManifold* next = manifold->getNext();
for (int i=index; (i+1) < mNbManifolds; i++) {
mManifolds[i] = mManifolds[i+1];
if (previous != nullptr) {
previous->setNext(next);
}
else {
mManifolds = next;
}
if (next != nullptr) {
next->setPrevious(previous);
}
// Delete the contact manifold
manifold->~ContactManifold();
mMemoryAllocator.release(manifold, sizeof(ContactManifold));
mNbManifolds--;
}
// Make all the contact manifolds and contact points obsolete
void ContactManifoldSet::makeContactsObsolete() {
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
manifold->setIsObsolete(true, true);
manifold = manifold->getNext();
}
}
// Clear the obsolete contact manifolds and contact points
void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
// Get the next manifold in the linked-list
ContactManifold* nextManifold = manifold->getNext();
// If the manifold is obsolete
if (manifold->getIsObsolete()) {
// Delete the contact manifold
removeManifold(manifold);
}
else {
// Clear the obsolete contact points of the manifold
manifold->clearObsoleteContactPoints();
}
manifold = nextManifold;
}
}
// Remove some contact manifolds and contact points if there are too many of them
void ContactManifoldSet::reduce() {
// Remove non optimal contact manifold while there are too many manifolds in the set
while (mNbManifolds > mNbMaxManifolds) {
removeNonOptimalManifold();
}
// Reduce all the contact manifolds in case they have too many contact points
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
manifold->reduce();
manifold = manifold->getNext();
}
}

88
src/collision/ContactManifoldSet.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -26,11 +26,16 @@
#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
// Libraries
#include "ContactManifold.h"
namespace reactphysics3d {
// Declarations
class ContactManifold;
class ContactManifoldInfo;
class ProxyShape;
class MemoryAllocator;
struct WorldSettings;
class CollisionShape;
// Constants
const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set
const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
@ -60,27 +65,37 @@ class ContactManifoldSet {
/// Pointer to the second proxy shape of the contact
ProxyShape* mShape2;
/// Reference to the memory allocator
/// Reference to the memory allocator for the contact manifolds
MemoryAllocator& mMemoryAllocator;
/// Contact manifolds of the set
ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
ContactManifold* mManifolds;
/// World settings
const WorldSettings& mWorldSettings;
// -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set
void createManifold(short normalDirectionId);
void createManifold(const ContactManifoldInfo* manifoldInfo);
/// Remove a contact manifold from the set
void removeManifold(int index);
// Return the contact manifold with a similar contact normal.
ContactManifold* selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const;
// Return the index of the contact manifold with a similar average normal.
int selectManifoldWithSimilarNormal(short int normalDirectionId) const;
/// Remove a contact manifold that is the least optimal (smaller penetration depth)
void removeNonOptimalManifold();
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int computeCubemapNormalId(const Vector3& normal) const;
/// Update a previous similar manifold with a new one
void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
/// Return the maximum number of contact manifolds allowed between to collision shapes
int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2);
/// Clear the contact manifold set
void clear();
/// Delete a contact manifold
void removeManifold(ContactManifold* manifold);
public:
@ -88,34 +103,37 @@ class ContactManifoldSet {
/// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds);
MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings);
/// Destructor
~ContactManifoldSet();
/// Add a contact manifold in the set
void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
/// Return the first proxy shape
ProxyShape* getShape1() const;
/// Return the second proxy shape
ProxyShape* getShape2() const;
/// Add a contact point to the manifold set
void addContactPoint(ContactPoint* contact);
/// Update the contact manifolds
void update();
/// Clear the contact manifold set
void clear();
/// Return the number of manifolds in the set
int getNbContactManifolds() const;
/// Return a given contact manifold
ContactManifold* getContactManifold(int index) const;
/// Return a pointer to the first element of the linked-list of contact manifolds
ContactManifold* getContactManifolds() const;
/// Make all the contact manifolds and contact points obsolete
void makeContactsObsolete();
/// Return the total number of contact points in the set of manifolds
int getTotalNbContactPoints() const;
/// Clear the obsolete contact manifolds and contact points
void clearObsoleteManifoldsAndContactPoints();
// Remove some contact manifolds and contact points if there are too many of them
void reduce();
};
// Return the first proxy shape
@ -133,19 +151,9 @@ inline int ContactManifoldSet::getNbContactManifolds() const {
return mNbManifolds;
}
// Return a given contact manifold
inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const {
assert(index >= 0 && index < mNbManifolds);
return mManifolds[index];
}
// Return the total number of contact points in the set of manifolds
inline int ContactManifoldSet::getTotalNbContactPoints() const {
int nbPoints = 0;
for (int i=0; i<mNbManifolds; i++) {
nbPoints += mManifolds[i]->getNbContactPoints();
}
return nbPoints;
// Return a pointer to the first element of the linked-list of contact manifolds
inline ContactManifold* ContactManifoldSet::getContactManifolds() const {
return mManifolds;
}
}

67
src/engine/Impulse.h → src/collision/ContactPointInfo.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -23,63 +23,68 @@
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_IMPULSE_H
#define REACTPHYSICS3D_IMPULSE_H
#ifndef REACTPHYSICS3D_CONTACT_POINT_INFO_H
#define REACTPHYSICS3D_CONTACT_POINT_INFO_H
// Libraries
#include "mathematics/mathematics.h"
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure Impulse
// Declarations
class CollisionBody;
// Structure ContactPointInfo
/**
* Represents an impulse that we can apply to bodies in the contact or constraint solver.
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
*/
struct Impulse {
struct ContactPointInfo {
private:
// -------------------- Methods -------------------- //
/// Private assignment operator
Impulse& operator=(const Impulse& impulse);
public:
// -------------------- Attributes -------------------- //
/// Linear impulse applied to the first body
const Vector3 linearImpulseBody1;
/// Normalized normal vector of the collision contact in world space
Vector3 normal;
/// Angular impulse applied to the first body
const Vector3 angularImpulseBody1;
/// Penetration depth of the contact
decimal penetrationDepth;
/// Linear impulse applied to the second body
const Vector3 linearImpulseBody2;
/// Contact point of body 1 in local space of body 1
Vector3 localPoint1;
/// Angular impulse applied to the second body
const Vector3 angularImpulseBody2;
/// Contact point of body 2 in local space of body 2
Vector3 localPoint2;
/// Pointer to the next contact point info
ContactPointInfo* next;
/// True if the contact point has already been inserted into a manifold
bool isUsed;
// -------------------- Methods -------------------- //
/// Constructor
Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1,
const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2)
: linearImpulseBody1(initLinearImpulseBody1),
angularImpulseBody1(initAngularImpulseBody1),
linearImpulseBody2(initLinearImpulseBody2),
angularImpulseBody2(initAngularImpulseBody2) {
ContactPointInfo(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2)
: normal(contactNormal), penetrationDepth(penDepth),
localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) {
assert(contactNormal.lengthSquare() > decimal(0.8));
assert(penDepth > decimal(0.0));
}
/// Copy-constructor
Impulse(const Impulse& impulse)
: linearImpulseBody1(impulse.linearImpulseBody1),
angularImpulseBody1(impulse.angularImpulseBody1),
linearImpulseBody2(impulse.linearImpulseBody2),
angularImpulseBody2(impulse.angularImpulseBody2) {
;
}
/// Destructor
~ContactPointInfo() = default;
};
}

118
src/collision/HalfEdgeStructure.cpp Normal file
View File

@ -0,0 +1,118 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "HalfEdgeStructure.h"
#include "containers/Map.h"
#include "containers/Pair.h"
#include "containers/containers_common.h"
using namespace reactphysics3d;
// Initialize the structure (when all vertices and faces have been added)
void HalfEdgeStructure::init() {
Map<VerticesPair, Edge> edges(mAllocator);
Map<VerticesPair, VerticesPair> nextEdges(mAllocator);
Map<VerticesPair, uint> mapEdgeToStartVertex(mAllocator);
Map<VerticesPair, uint> mapEdgeToIndex(mAllocator);
Map<uint, VerticesPair> mapEdgeIndexToKey(mAllocator);
Map<uint, VerticesPair> mapFaceIndexToEdgeKey(mAllocator);
List<VerticesPair> currentFaceEdges(mAllocator, mFaces[0].faceVertices.size());
// For each face
for (uint f=0; f<mFaces.size(); f++) {
Face face = mFaces[f];
VerticesPair firstEdgeKey(0, 0);
// For each vertex of the face
for (uint v=0; v < face.faceVertices.size(); v++) {
uint v1Index = face.faceVertices[v];
uint v2Index = face.faceVertices[v == (face.faceVertices.size() - 1) ? 0 : v + 1];
const VerticesPair pairV1V2 = VerticesPair(v1Index, v2Index);
// Create a new half-edge
Edge edge;
edge.faceIndex = f;
edge.vertexIndex = v1Index;
if (v == 0) {
firstEdgeKey = pairV1V2;
}
else if (v >= 1) {
nextEdges.add(Pair<VerticesPair, VerticesPair>(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2));
}
if (v == (face.faceVertices.size() - 1)) {
nextEdges.add(Pair<VerticesPair, VerticesPair>(pairV1V2, firstEdgeKey));
}
edges.add(Pair<VerticesPair, Edge>(pairV1V2, edge));
const VerticesPair pairV2V1(v2Index, v1Index);
mapEdgeToStartVertex.add(Pair<VerticesPair, uint>(pairV1V2, v1Index), true);
mapEdgeToStartVertex.add(Pair<VerticesPair, uint>(pairV2V1, v2Index), true);
mapFaceIndexToEdgeKey.add(Pair<uint, VerticesPair>(f, pairV1V2), true);
auto itEdge = edges.find(pairV2V1);
if (itEdge != edges.end()) {
const uint edgeIndex = mEdges.size();
itEdge->second.twinEdgeIndex = edgeIndex + 1;
edge.twinEdgeIndex = edgeIndex;
mapEdgeIndexToKey.add(Pair<uint, VerticesPair>(edgeIndex, pairV2V1));
mapEdgeIndexToKey.add(Pair<uint, VerticesPair>(edgeIndex + 1, pairV1V2));
mVertices[v1Index].edgeIndex = edgeIndex + 1;
mVertices[v2Index].edgeIndex = edgeIndex;
mapEdgeToIndex.add(Pair<VerticesPair, uint>(pairV1V2, edgeIndex + 1));
mapEdgeToIndex.add(Pair<VerticesPair, uint>(pairV2V1, edgeIndex));
mEdges.add(itEdge->second);
mEdges.add(edge);
}
currentFaceEdges.add(pairV1V2);
}
currentFaceEdges.clear();
}
// Set next edges
for (uint i=0; i < mEdges.size(); i++) {
mEdges[i].nextEdgeIndex = mapEdgeToIndex[nextEdges[mapEdgeIndexToKey[i]]];
}
// Set face edge
for (uint f=0; f < mFaces.size(); f++) {
mFaces[f].edgeIndex = mapEdgeToIndex[mapFaceIndexToEdgeKey[f]];
}
}

204
src/collision/HalfEdgeStructure.h Normal file
View File

@ -0,0 +1,204 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H
#define REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H
// Libraries
#include "mathematics/mathematics.h"
namespace reactphysics3d {
// Class HalfEdgeStructure
/**
* This class describes a polyhedron mesh made of faces and vertices.
* The faces do not have to be triangle. Note that the half-edge structure
* is only valid if the mesh is closed (each edge has two adjacent faces).
*/
class HalfEdgeStructure {
public:
using VerticesPair = Pair<uint, uint>;
/// Edge
struct Edge {
uint vertexIndex; // Index of the vertex at the beginning of the edge
uint twinEdgeIndex; // Index of the twin edge
uint faceIndex; // Adjacent face index of the edge
uint nextEdgeIndex; // Index of the next edge
};
/// Face
struct Face {
uint edgeIndex; // Index of an half-edge of the face
List<uint> faceVertices; // Index of the vertices of the face
/// Constructor
Face(MemoryAllocator& allocator) : faceVertices(allocator) {}
/// Constructor
Face(List<uint> vertices) : faceVertices(vertices) {}
};
/// Vertex
struct Vertex {
uint vertexPointIndex; // Index of the vertex point in the origin vertex array
uint edgeIndex; // Index of one edge emanting from this vertex
/// Constructor
Vertex(uint vertexCoordsIndex) : vertexPointIndex(vertexCoordsIndex) { }
};
private:
/// Reference to a memory allocator
MemoryAllocator& mAllocator;
/// All the faces
List<Face> mFaces;
/// All the vertices
List<Vertex> mVertices;
/// All the half-edges
List<Edge> mEdges;
public:
/// Constructor
HalfEdgeStructure(MemoryAllocator& allocator, uint facesCapacity, uint verticesCapacity,
uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity),
mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {}
/// Destructor
~HalfEdgeStructure() = default;
/// Initialize the structure (when all vertices and faces have been added)
void init();
/// Add a vertex
uint addVertex(uint vertexPointIndex);
/// Add a face
void addFace(List<uint> faceVertices);
/// Return the number of faces
uint getNbFaces() const;
/// Return the number of half-edges
uint getNbHalfEdges() const;
/// Return the number of vertices
uint getNbVertices() const;
/// Return a given face
const Face& getFace(uint index) const;
/// Return a given edge
const Edge& getHalfEdge(uint index) const;
/// Return a given vertex
const Vertex& getVertex(uint index) const;
};
// Add a vertex
/**
* @param vertexPointIndex Index of the vertex in the vertex data array
*/
inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) {
Vertex vertex(vertexPointIndex);
mVertices.add(vertex);
return mVertices.size() - 1;
}
// Add a face
/**
* @param faceVertices List of the vertices in a face (ordered in CCW order as seen from outside
* the polyhedron
*/
inline void HalfEdgeStructure::addFace(List<uint> faceVertices) {
// Create a new face
Face face(faceVertices);
mFaces.add(face);
}
// Return the number of faces
/**
* @return The number of faces in the polyhedron
*/
inline uint HalfEdgeStructure::getNbFaces() const {
return static_cast<uint>(mFaces.size());
}
// Return the number of edges
/**
* @return The number of edges in the polyhedron
*/
inline uint HalfEdgeStructure::getNbHalfEdges() const {
return static_cast<uint>(mEdges.size());
}
// Return the number of vertices
/**
* @return The number of vertices in the polyhedron
*/
inline uint HalfEdgeStructure::getNbVertices() const {
return static_cast<uint>(mVertices.size());
}
// Return a given face
/**
* @return A given face of the polyhedron
*/
inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) const {
assert(index < mFaces.size());
return mFaces[index];
}
// Return a given edge
/**
* @return A given edge of the polyhedron
*/
inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) const {
assert(index < mEdges.size());
return mEdges[index];
}
// Return a given vertex
/**
* @return A given vertex of the polyhedron
*/
inline const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint index) const {
assert(index < mVertices.size());
return mVertices[index];
}
}
#endif

63
src/collision/MiddlePhaseTriangleCallback.cpp Normal file
View File

@ -0,0 +1,63 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "collision/MiddlePhaseTriangleCallback.h"
#include "engine/OverlappingPair.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/shapes/TriangleShape.h"
using namespace reactphysics3d;
// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
// Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
// destructor of the corresponding NarrowPhaseInfo.
TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
TriangleShape(trianglePoints, verticesNormals, shapeId, mAllocator);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler to the triangle shape
triangleShape->setProfiler(mProfiler);
#endif
bool isShape1Convex = mOverlappingPair->getShape1()->getCollisionShape()->isConvex();
ProxyShape* shape1 = isShape1Convex ? mConvexProxyShape : mConcaveProxyShape;
ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape;
// Create a narrow phase info for the narrow-phase collision detection
NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo)))
NarrowPhaseInfo(mOverlappingPair,
isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape,
isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(),
shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(),
mAllocator);
narrowPhaseInfoList->next = firstNarrowPhaseInfo;
}

111
src/collision/MiddlePhaseTriangleCallback.h Normal file
View File

@ -0,0 +1,111 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H
#define REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H
#include "configuration.h"
#include "collision/shapes/ConcaveShape.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Libraries
// Declarations
class ConcaveShape;
class OverlappingPair;
class NarrowPhaseAlgorithm;
class ProxyShape;
class MemoryAllocator;
class Profiler;
struct NarrowPhaseInfo;
struct Vector3;
// Class ConvexVsTriangleCallback
/**
* This class is used to report a collision between the triangle
* of a concave mesh shape and a convex shape during the
* middle-phase algorithm.
*/
class MiddlePhaseTriangleCallback : public TriangleCallback {
protected:
/// Broadphase overlapping pair
OverlappingPair* mOverlappingPair;
/// Pointer to the concave proxy shape
ProxyShape* mConcaveProxyShape;
/// Pointer to the convex proxy shape
ProxyShape* mConvexProxyShape;
/// Pointer to the concave collision shape
const ConcaveShape* mConcaveShape;
/// Reference to the single-frame memory allocator
MemoryAllocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public:
/// Pointer to the first element of the linked-list of narrow-phase info
NarrowPhaseInfo* narrowPhaseInfoList;
/// Constructor
MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
ProxyShape* concaveProxyShape,
ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
MemoryAllocator& allocator)
:mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
mAllocator(allocator), narrowPhaseInfoList(nullptr) {
}
/// Test collision between a triangle and the convex mesh shape
virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
};
}
#endif

109
src/collision/NarrowPhaseInfo.cpp Normal file
View File

@ -0,0 +1,109 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "NarrowPhaseInfo.h"
#include "ContactPointInfo.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/OverlappingPair.h"
using namespace reactphysics3d;
// Constructor
NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, MemoryAllocator& shapeAllocator)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
contactPoints(nullptr), next(nullptr), collisionShapeAllocator(shapeAllocator) {
// Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
}
// Destructor
NarrowPhaseInfo::~NarrowPhaseInfo() {
assert(contactPoints == nullptr);
// Release the memory of the TriangleShape (this memory was allocated in the
// MiddlePhaseTriangleCallback::testTriangle() method)
if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) {
collisionShape1->~CollisionShape();
collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape));
}
if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) {
collisionShape2->~CollisionShape();
collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape));
}
}
// Add a new contact point
void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
assert(penDepth > decimal(0.0));
// Get the memory allocator
MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator();
// Create the contact point info
ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
// Add it into the linked list of contact points
contactPointInfo->next = contactPoints;
contactPoints = contactPointInfo;
}
/// Take all the generated contact points and create a new potential
/// contact manifold into the overlapping pair
void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() {
overlappingPair->addPotentialContactPoints(this);
}
// Reset the remaining contact points
void NarrowPhaseInfo::resetContactPoints() {
// Get the memory allocator
MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator();
// For each remaining contact point info
ContactPointInfo* element = contactPoints;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Call the destructor
elementToDelete->~ContactPointInfo();
// Delete the current element
allocator.release(elementToDelete, sizeof(ContactPointInfo));
}
contactPoints = nullptr;
}

104
src/collision/NarrowPhaseInfo.h Normal file
View File

@ -0,0 +1,104 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_NARROW_PHASE_INFO_H
#define REACTPHYSICS3D_NARROW_PHASE_INFO_H
// Libraries
#include "engine/OverlappingPair.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class CollisionShape;
struct LastFrameCollisionInfo;
class ContactManifoldInfo;
struct ContactPointInfo;
// Class NarrowPhaseInfo
/**
* This structure regroups different things about a collision shape. This is
* used to pass information about a collision shape to a collision algorithm.
*/
struct NarrowPhaseInfo {
public:
/// Broadphase overlapping pair
OverlappingPair* overlappingPair;
/// Pointer to the first collision shape to test collision with
CollisionShape* collisionShape1;
/// Pointer to the second collision shape to test collision with
CollisionShape* collisionShape2;
/// Transform that maps from collision shape 1 local-space to world-space
Transform shape1ToWorldTransform;
/// Transform that maps from collision shape 2 local-space to world-space
Transform shape2ToWorldTransform;
/// Linked-list of contact points created during the narrow-phase
ContactPointInfo* contactPoints;
/// Pointer to the next element in the linked list
NarrowPhaseInfo* next;
/// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
MemoryAllocator& collisionShapeAllocator;
/// Constructor
NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, MemoryAllocator& shapeAllocator);
/// Destructor
~NarrowPhaseInfo();
/// Add a new contact point
void addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2);
/// Create a new potential contact manifold into the overlapping pair using current contact points
void addContactPointsAsPotentialContactManifold();
/// Reset the remaining contact points
void resetContactPoints();
/// Get the last collision frame info for temporal coherence
LastFrameCollisionInfo* getLastFrameCollisionInfo() const;
};
// Get the last collision frame info for temporal coherence
inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const {
return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId());
}
}
#endif

57
src/collision/OverlapCallback.h Normal file
View File

@ -0,0 +1,57 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_OVERLAP_CALLBACK_H
#define REACTPHYSICS3D_OVERLAP_CALLBACK_H
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class CollisionBody;
// Class OverlapCallback
/**
* This class can be used to register a callback for collision overlap queries.
* You should implement your own class inherited from this one and implement
* the notifyOverlap() method. This method will called each time a contact
* point is reported.
*/
class OverlapCallback {
public:
/// Destructor
virtual ~OverlapCallback() {
}
/// This method will be called for each reported overlapping bodies
virtual void notifyOverlap(CollisionBody* collisionBody)=0;
};
}
#endif

87
src/collision/PolygonVertexArray.cpp Normal file
View File

@ -0,0 +1,87 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "PolygonVertexArray.h"
using namespace reactphysics3d;
// Constructor
/// Note that your data will not be copied into the PolygonVertexArray and
/// therefore, you need to make sure that those data are always valid during
/// the lifetime of the PolygonVertexArray.
/**
* @param nbVertices Number of vertices in the array
* @param verticesStart Pointer to the start of the vertices data
* @param verticesStride The number of bytes between two consecutive vertices in the array
* @param indexesStart Pointer to the start of the face indices data
* @param indexesStride The number of bytes between two consecutive face indices in the array
* @param nbFaces The number of faces in the array
* @param nbFaces Pointer to the start of the faces data
* @param vertexDataType Data type of the vertices data
* @param indexDataType Data type of the face indices data
*/
PolygonVertexArray::PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
void* indexesStart, int indexesStride,
uint nbFaces, PolygonFace* facesStart,
VertexDataType vertexDataType, IndexDataType indexDataType) {
mNbVertices = nbVertices;
mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
mVerticesStride = verticesStride;
mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
mIndicesStride = indexesStride;
mNbFaces = nbFaces;
mPolygonFacesStart = facesStart;
mVertexDataType = vertexDataType;
mIndexDataType = indexDataType;
}
// Return the vertex index of a given vertex in a face
/**
* @return The index of a vertex (in the array of vertices data) of a given vertex in a face
*/
uint PolygonVertexArray::getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const {
assert(faceIndex < mNbFaces);
// Get the face
PolygonFace* face = getPolygonFace(faceIndex);
assert(noVertexInFace < face->nbVertices);
void* vertexIndexPointer = mIndicesStart + (face->indexBase + noVertexInFace) * mIndicesStride;
if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
return *((uint*)vertexIndexPointer);
}
else if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
return *((unsigned short*)vertexIndexPointer);
}
else {
assert(false);
}
return 0;
}

216
src/collision/PolygonVertexArray.h Normal file
View File

@ -0,0 +1,216 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H
#define REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H
// Libraries
#include "configuration.h"
#include <cassert>
namespace reactphysics3d {
// Class PolygonVertexArray
/**
* This class is used to describe the vertices and faces of a polyhedron mesh.
* A PolygonVertexArray represents an array of vertices and polygon faces
* of a polyhedron mesh. When you create a PolygonVertexArray, no data is copied
* into the array. It only stores pointer to the data. The purpose is to allow
* the user to share vertices data between the physics engine and the rendering
* part. Therefore, make sure that the data pointed by a PolygonVertexArray
* remains valid during the PolygonVertexArray life.
*/
class PolygonVertexArray {
public:
/// Data type for the vertices in the array
enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
/// Data type for the indices in the array
enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
/// Represent a polygon face of the polyhedron
struct PolygonFace {
/// Number of vertices in the polygon face
uint nbVertices;
/// Index of the first vertex of the polygon face
/// inside the array with all vertex indices
uint indexBase;
};
protected:
/// Number of vertices in the array
uint mNbVertices;
/// Pointer to the first vertex value in the array
unsigned char* mVerticesStart;
/// Stride (number of bytes) between the beginning of two vertices
/// values in the array
int mVerticesStride;
/// Pointer to the first vertex index of the array
unsigned char* mIndicesStart;
/// Stride (number of bytes) between the beginning of two indices in
/// the array
int mIndicesStride;
/// Number of polygon faces in the array
uint mNbFaces;
/// Pointer to the first polygon face in the polyhedron
PolygonFace* mPolygonFacesStart;
/// Data type of the vertices in the array
VertexDataType mVertexDataType;
/// Data type of the indices in the array
IndexDataType mIndexDataType;
public:
/// Constructor
PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
void* indexesStart, int indexesStride,
uint nbFaces, PolygonFace* facesStart,
VertexDataType vertexDataType, IndexDataType indexDataType);
/// Destructor
~PolygonVertexArray() = default;
/// Return the vertex data type
VertexDataType getVertexDataType() const;
/// Return the index data type
IndexDataType getIndexDataType() const;
/// Return the number of vertices
uint getNbVertices() const;
/// Return the number of faces
uint getNbFaces() const;
/// Return the vertices stride (number of bytes)
int getVerticesStride() const;
/// Return the indices stride (number of bytes)
int getIndicesStride() const;
/// Return the vertex index of a given vertex in a face
uint getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const;
/// Return a polygon face of the polyhedron
PolygonFace* getPolygonFace(uint faceIndex) const;
/// Return the pointer to the start of the vertices array
unsigned char* getVerticesStart() const;
/// Return the pointer to the start of the indices array
unsigned char* getIndicesStart() const;
};
// Return the vertex data type
/**
* @return The data type of the vertices in the array
*/
inline PolygonVertexArray::VertexDataType PolygonVertexArray::getVertexDataType() const {
return mVertexDataType;
}
// Return the index data type
/**
* @return The data type of the indices in the array
*/
inline PolygonVertexArray::IndexDataType PolygonVertexArray::getIndexDataType() const {
return mIndexDataType;
}
// Return the number of vertices
/**
* @return The number of vertices in the array
*/
inline uint PolygonVertexArray::getNbVertices() const {
return mNbVertices;
}
// Return the number of faces
/**
* @return The number of faces in the array
*/
inline uint PolygonVertexArray::getNbFaces() const {
return mNbFaces;
}
// Return the vertices stride (number of bytes)
/**
* @return The number of bytes between two vertices
*/
inline int PolygonVertexArray::getVerticesStride() const {
return mVerticesStride;
}
// Return the indices stride (number of bytes)
/**
* @return The number of bytes between two consecutive face indices
*/
inline int PolygonVertexArray::getIndicesStride() const {
return mIndicesStride;
}
// Return a polygon face of the polyhedron
/**
* @param faceIndex Index of a given face
* @return A polygon face
*/
inline PolygonVertexArray::PolygonFace* PolygonVertexArray::getPolygonFace(uint faceIndex) const {
assert(faceIndex < mNbFaces);
return &mPolygonFacesStart[faceIndex];
}
// Return the pointer to the start of the vertices array
/**
* @return A pointer to the start of the vertex array of the polyhedron
*/
inline unsigned char* PolygonVertexArray::getVerticesStart() const {
return mVerticesStart;
}
// Return the pointer to the start of the indices array
/**
* @return A pointer to the start of the face indices array of the polyhedron
*/
inline unsigned char* PolygonVertexArray::getIndicesStart() const {
return mIndicesStart;
}
}
#endif

157
src/collision/PolyhedronMesh.cpp Normal file
View File

@ -0,0 +1,157 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "PolyhedronMesh.h"
#include "memory/MemoryManager.h"
#include "collision/PolygonVertexArray.h"
using namespace reactphysics3d;
// Constructor
/**
* Create a polyhedron mesh given an array of polygons.
* @param polygonVertexArray Pointer to the array of polygons and their vertices
*/
PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray)
: mHalfEdgeStructure(MemoryManager::getBaseAllocator(),
polygonVertexArray->getNbFaces(),
polygonVertexArray->getNbVertices(),
(polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) {
mPolygonVertexArray = polygonVertexArray;
// Create the half-edge structure of the mesh
createHalfEdgeStructure();
// Create the face normals array
mFacesNormals = new Vector3[mHalfEdgeStructure.getNbFaces()];
// Compute the faces normals
computeFacesNormals();
// Compute the centroid
computeCentroid();
}
// Destructor
PolyhedronMesh::~PolyhedronMesh() {
delete[] mFacesNormals;
}
// Create the half-edge structure of the mesh
void PolyhedronMesh::createHalfEdgeStructure() {
// For each vertex of the mesh
for (uint v=0; v < mPolygonVertexArray->getNbVertices(); v++) {
mHalfEdgeStructure.addVertex(v);
}
// For each polygon face of the mesh
for (uint f=0; f < mPolygonVertexArray->getNbFaces(); f++) {
// Get the polygon face
PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f);
List<uint> faceVertices(MemoryManager::getBaseAllocator(), face->nbVertices);
// For each vertex of the face
for (uint v=0; v < face->nbVertices; v++) {
faceVertices.add(mPolygonVertexArray->getVertexIndexInFace(f, v));
}
assert(faceVertices.size() >= 3);
// Addd the face into the half-edge structure
mHalfEdgeStructure.addFace(faceVertices);
}
// Initialize the half-edge structure
mHalfEdgeStructure.init();
}
/// Return a vertex
/**
* @param index Index of a given vertex in the mesh
* @return The coordinates of a given vertex in the mesh
*/
Vector3 PolyhedronMesh::getVertex(uint index) const {
assert(index < getNbVertices());
// Get the vertex index in the array with all vertices
uint vertexIndex = mHalfEdgeStructure.getVertex(index).vertexPointIndex;
PolygonVertexArray::VertexDataType vertexType = mPolygonVertexArray->getVertexDataType();
unsigned char* verticesStart = mPolygonVertexArray->getVerticesStart();
int vertexStride = mPolygonVertexArray->getVerticesStride();
Vector3 vertex;
if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
vertex.x = decimal(vertices[0]);
vertex.y = decimal(vertices[1]);
vertex.z = decimal(vertices[2]);
}
else if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
vertex.x = decimal(vertices[0]);
vertex.y = decimal(vertices[1]);
vertex.z = decimal(vertices[2]);
}
else {
assert(false);
}
return vertex;
}
// Compute the faces normals
void PolyhedronMesh::computeFacesNormals() {
// For each face
for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) {
const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f);
assert(face.faceVertices.size() >= 3);
const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]);
const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]);
mFacesNormals[f] = vec1.cross(vec2);
mFacesNormals[f].normalize();
}
}
// Compute the centroid of the polyhedron
void PolyhedronMesh::computeCentroid() {
mCentroid.setToZero();
for (uint v=0; v < getNbVertices(); v++) {
mCentroid += getVertex(v);
}
mCentroid /= getNbVertices();
}

148
src/collision/PolyhedronMesh.h Normal file
View File

@ -0,0 +1,148 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_POLYHEDRON_MESH_H
#define REACTPHYSICS3D_POLYHEDRON_MESH_H
// Libraries
#include "mathematics/mathematics.h"
#include "HalfEdgeStructure.h"
namespace reactphysics3d {
// Declarations
class DefaultAllocator;
class PolygonVertexArray;
// Class PolyhedronMesh
/**
* This class describes a polyhedron mesh made of faces and vertices.
* The faces do not have to be triangles.
*/
class PolyhedronMesh {
private:
// -------------------- Attributes -------------------- //
/// Pointer the the polygon vertex array with vertices and faces
/// of the mesh
PolygonVertexArray* mPolygonVertexArray;
/// Half-edge structure of the mesh
HalfEdgeStructure mHalfEdgeStructure;
/// Array with the face normals
Vector3* mFacesNormals;
/// Centroid of the polyhedron
Vector3 mCentroid;
// -------------------- Methods -------------------- //
/// Create the half-edge structure of the mesh
void createHalfEdgeStructure();
/// Compute the faces normals
void computeFacesNormals();
/// Compute the centroid of the polyhedron
void computeCentroid() ;
public:
// -------------------- Methods -------------------- //
/// Constructor
PolyhedronMesh(PolygonVertexArray* polygonVertexArray);
/// Destructor
~PolyhedronMesh();
/// Return the number of vertices
uint getNbVertices() const;
/// Return a vertex
Vector3 getVertex(uint index) const;
/// Return the number of faces
uint getNbFaces() const;
/// Return a face normal
Vector3 getFaceNormal(uint faceIndex) const;
/// Return the half-edge structure of the mesh
const HalfEdgeStructure& getHalfEdgeStructure() const;
/// Return the centroid of the polyhedron
Vector3 getCentroid() const;
};
// Return the number of vertices
/**
* @return The number of vertices in the mesh
*/
inline uint PolyhedronMesh::getNbVertices() const {
return mHalfEdgeStructure.getNbVertices();
}
// Return the number of faces
/**
* @return The number of faces in the mesh
*/
inline uint PolyhedronMesh::getNbFaces() const {
return mHalfEdgeStructure.getNbFaces();
}
// Return a face normal
/**
* @param faceIndex The index of a given face of the mesh
* @return The normal vector of a given face of the mesh
*/
inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const {
assert(faceIndex < mHalfEdgeStructure.getNbFaces());
return mFacesNormals[faceIndex];
}
// Return the half-edge structure of the mesh
/**
* @return The Half-Edge structure of the mesh
*/
inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const {
return mHalfEdgeStructure;
}
// Return the centroid of the polyhedron
/**
* @return The centroid of the mesh
*/
inline Vector3 PolyhedronMesh::getCentroid() const {
return mCentroid;
}
}
#endif

59
src/collision/ProxyShape.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -25,6 +25,9 @@
// Libraries
#include "ProxyShape.h"
#include "utils/Logger.h"
#include "collision/RaycastInfo.h"
#include "memory/MemoryManager.h"
using namespace reactphysics3d;
@ -35,20 +38,15 @@ using namespace reactphysics3d;
* @param transform Transformation from collision shape local-space to body local-space
* @param mass Mass of the collision shape (in kilograms)
*/
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL),
mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, MemoryManager& memoryManager)
:mMemoryManager(memoryManager), mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(nullptr), mBroadPhaseID(-1), mUserData(nullptr), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
}
// Destructor
ProxyShape::~ProxyShape() {
// Release the cached collision data memory
if (mCachedCollisionData != NULL) {
free(mCachedCollisionData);
}
}
// Return true if a point is inside the collision shape
@ -62,12 +60,51 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) {
return mCollisionShape->testPointInside(localPoint, this);
}
// Set the collision category bits
/**
* @param collisionCategoryBits The collision category bits mask of the proxy shape
*/
void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) {
mCollisionCategoryBits = collisionCategoryBits;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape,
"ProxyShape " + std::to_string(mBroadPhaseID) + ": Set collisionCategoryBits=" +
std::to_string(mCollisionCategoryBits));
}
// Set the collision bits mask
/**
* @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide
*/
void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
mCollideWithMaskBits = collideWithMaskBits;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape,
"ProxyShape " + std::to_string(mBroadPhaseID) + ": Set collideWithMaskBits=" +
std::to_string(mCollideWithMaskBits));
}
// Set the local to parent body transform
void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
mLocalToBodyTransform = transform;
mBody->setIsSleeping(false);
// Notify the body that the proxy shape has to be updated in the broad-phase
mBody->updateProxyShapeInBroadPhase(this, true);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape,
"ProxyShape " + std::to_string(mBroadPhaseID) + ": Set localToBodyTransform=" +
mLocalToBodyTransform.to_string());
}
// Raycast method with feedback information
/**
* @param ray Ray to use for the raycasting
* @param[out] raycastInfo Result of the raycasting that is valid only if the
* methods returned true
* @return True if the ray hit the collision shape
* @return True if the ray hits the collision shape
*/
bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
@ -81,7 +118,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
worldToLocalTransform * ray.point2,
ray.maxFraction);
bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mMemoryManager.getPoolAllocator());
// Convert the raycast info into world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;

160
src/collision/ProxyShape.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -28,10 +28,13 @@
// Libraries
#include "body/CollisionBody.h"
#include "shapes/CollisionShape.h"
#include "collision/shapes/CollisionShape.h"
namespace reactphysics3d {
// Declarations
class MemoryManager;
// Class ProxyShape
/**
* The CollisionShape instances are supposed to be unique for memory optimization. For instance,
@ -48,6 +51,9 @@ class ProxyShape {
// -------------------- Attributes -------------------- //
/// Reference to the memory manager
MemoryManager& mMemoryManager;
/// Pointer to the parent body
CollisionBody* mBody;
@ -66,9 +72,6 @@ class ProxyShape {
/// Broad-phase ID (node ID in the dynamic AABB tree)
int mBroadPhaseID;
/// Cached collision data
void* mCachedCollisionData;
/// Pointer to user data
void* mUserData;
@ -85,13 +88,23 @@ class ProxyShape {
/// proxy shape will collide with every collision categories by default.
unsigned short mCollideWithMaskBits;
// -------------------- Methods -------------------- //
#ifdef IS_PROFILING_ACTIVE
/// Private copy-constructor
ProxyShape(const ProxyShape& proxyShape);
/// Pointer to the profiler
Profiler* mProfiler;
/// Private assignment operator
ProxyShape& operator=(const ProxyShape& proxyShape);
#endif
#ifdef IS_LOGGING_ACTIVE
/// Logger
Logger* mLogger;
#endif
// -------------------- Methods -------------------- //
/// Return the collision shape
CollisionShape* getCollisionShape();
public:
@ -99,10 +112,16 @@ class ProxyShape {
/// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape,
const Transform& transform, decimal mass);
const Transform& transform, decimal mass, MemoryManager& memoryManager);
/// Destructor
~ProxyShape();
virtual ~ProxyShape();
/// Deleted copy-constructor
ProxyShape(const ProxyShape& proxyShape) = delete;
/// Deleted assignment operator
ProxyShape& operator=(const ProxyShape& proxyShape) = delete;
/// Return the collision shape
const CollisionShape* getCollisionShape() const;
@ -128,6 +147,12 @@ class ProxyShape {
/// Return the local to world transform
const Transform getLocalToWorldTransform() const;
/// Return the AABB of the proxy shape in world-space
const AABB getWorldAABB() const;
/// Test if the proxy shape overlaps with a given AABB
bool testAABBOverlap(const AABB& worldAABB) const;
/// Return true if a point is inside the collision shape
bool testPointInside(const Vector3& worldPoint);
@ -152,14 +177,21 @@ class ProxyShape {
/// Return the next proxy shape in the linked list of proxy shapes
const ProxyShape* getNext() const;
/// Return the pointer to the cached collision data
void** getCachedCollisionData();
/// Return the broad-phase id
int getBroadPhaseId() const;
/// Return the local scaling vector of the collision shape
Vector3 getLocalScaling() const;
#ifdef IS_PROFILING_ACTIVE
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
#ifdef IS_LOGGING_ACTIVE
/// Set the logger
void setLogger(Logger* logger);
#endif
// -------------------- Friendship -------------------- //
@ -171,17 +203,13 @@ class ProxyShape {
friend class CollisionDetection;
friend class CollisionWorld;
friend class DynamicsWorld;
friend class EPAAlgorithm;
friend class GJKAlgorithm;
friend class ConvexMeshShape;
friend class ContactManifoldSet;
friend class MiddlePhaseTriangleCallback;
};
// Return the pointer to the cached collision data
inline void** ProxyShape::getCachedCollisionData() {
return &mCachedCollisionData;
}
// Return the collision shape
/**
* @return Pointer to the internal collision shape
@ -190,6 +218,14 @@ inline const CollisionShape* ProxyShape::getCollisionShape() const {
return mCollisionShape;
}
// Return the collision shape
/**
* @return Pointer to the internal collision shape
*/
inline CollisionShape* ProxyShape::getCollisionShape() {
return mCollisionShape;
}
// Return the parent body
/**
* @return Pointer to the parent body
@ -231,17 +267,6 @@ inline const Transform& ProxyShape::getLocalToBodyTransform() const {
return mLocalToBodyTransform;
}
// Set the local to parent body transform
inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
mLocalToBodyTransform = transform;
mBody->setIsSleeping(false);
// Notify the body that the proxy shape has to be updated in the broad-phase
mBody->updateProxyShapeInBroadPhase(this, true);
}
// Return the local to world transform
/**
* @return The transformation that transforms the local-space of the collision
@ -251,6 +276,16 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const {
return mBody->mTransform * mLocalToBodyTransform;
}
// Return the AABB of the proxy shape in world-space
/**
* @return The AABB of the proxy shape in world-space
*/
inline const AABB ProxyShape::getWorldAABB() const {
AABB aabb;
mCollisionShape->computeAABB(aabb, getLocalToWorldTransform());
return aabb;
}
// Return the next proxy shape in the linked list of proxy shapes
/**
* @return Pointer to the next proxy shape in the linked list of proxy shapes
@ -275,14 +310,6 @@ inline unsigned short ProxyShape::getCollisionCategoryBits() const {
return mCollisionCategoryBits;
}
// Set the collision category bits
/**
* @param collisionCategoryBits The collision category bits mask of the proxy shape
*/
inline void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) {
mCollisionCategoryBits = collisionCategoryBits;
}
// Return the collision bits mask
/**
* @return The bits mask that specifies with which collision category this shape will collide
@ -291,37 +318,42 @@ inline unsigned short ProxyShape::getCollideWithMaskBits() const {
return mCollideWithMaskBits;
}
// Set the collision bits mask
/**
* @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide
*/
inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
mCollideWithMaskBits = collideWithMaskBits;
// Return the broad-phase id
inline int ProxyShape::getBroadPhaseId() const {
return mBroadPhaseID;
}
// Return the local scaling vector of the collision shape
/// Test if the proxy shape overlaps with a given AABB
/**
* @return The local scaling vector
*/
inline Vector3 ProxyShape::getLocalScaling() const {
return mCollisionShape->getScaling();
* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap
* @return True if the given AABB overlaps with the AABB of the collision body
*/
inline bool ProxyShape::testAABBOverlap(const AABB& worldAABB) const {
return worldAABB.testCollision(getWorldAABB());
}
// Set the local scaling vector of the collision shape
/**
* @param scaling The new local scaling vector
*/
inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
#ifdef IS_PROFILING_ACTIVE
// Set the local scaling of the collision shape
mCollisionShape->setLocalScaling(scaling);
// Set the profiler
inline void ProxyShape::setProfiler(Profiler* profiler) {
mBody->setIsSleeping(false);
mProfiler = profiler;
// Notify the body that the proxy shape has to be updated in the broad-phase
mBody->updateProxyShapeInBroadPhase(this, true);
mCollisionShape->setProfiler(profiler);
}
#endif
#ifdef IS_LOGGING_ACTIVE
// Set the logger
inline void ProxyShape::setLogger(Logger* logger) {
mLogger = logger;
}
#endif
}
#endif

2
src/collision/RaycastInfo.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *

22
src/collision/RaycastInfo.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -28,7 +28,6 @@
// Libraries
#include "mathematics/Vector3.h"
#include "mathematics/Ray.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -37,6 +36,7 @@ namespace reactphysics3d {
class CollisionBody;
class ProxyShape;
class CollisionShape;
struct Ray;
// Structure RaycastInfo
/**
@ -46,14 +46,6 @@ struct RaycastInfo {
private:
// -------------------- Methods -------------------- //
/// Private copy constructor
RaycastInfo(const RaycastInfo& raycastInfo);
/// Private assignment operator
RaycastInfo& operator=(const RaycastInfo& raycastInfo);
public:
// -------------------- Attributes -------------------- //
@ -83,14 +75,18 @@ struct RaycastInfo {
// -------------------- Methods -------------------- //
/// Constructor
RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) {
RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(nullptr), proxyShape(nullptr) {
}
/// Destructor
~RaycastInfo() {
~RaycastInfo() = default;
}
/// Deleted copy constructor
RaycastInfo(const RaycastInfo& raycastInfo) = delete;
/// Deleted assignment operator
RaycastInfo& operator=(const RaycastInfo& raycastInfo) = delete;
};
// Class RaycastCallback

11
src/collision/TriangleMesh.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -25,15 +25,12 @@
// Libraries
#include "TriangleMesh.h"
#include "memory/MemoryManager.h"
using namespace reactphysics3d;
// Constructor
TriangleMesh::TriangleMesh() {
}
// Destructor
TriangleMesh::~TriangleMesh() {
TriangleMesh::TriangleMesh()
: mTriangleArrays(MemoryManager::getBaseAllocator()) {
}

29
src/collision/TriangleMesh.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,18 +27,21 @@
#define REACTPHYSICS3D_TRIANGLE_MESH_H
// Libraries
#include <vector>
#include <cassert>
#include "TriangleVertexArray.h"
#include "containers/List.h"
namespace reactphysics3d {
// Declarations
class TriangleVertexArray;
class MemoryManager;
// Class TriangleMesh
/**
* This class represents a mesh made of triangles. A TriangleMesh contains
* one or several parts. Each part is a set of triangles represented in a
* TriangleVertexArray object describing all the triangles vertices of the part.
* A TriangleMesh object is used to create a ConcaveMeshShape from a triangle
* A TriangleMesh object can be used to create a ConcaveMeshShape from a triangle
* mesh for instance.
*/
class TriangleMesh {
@ -46,7 +49,7 @@ class TriangleMesh {
protected:
/// All the triangle arrays of the mesh (one triangle array per part)
std::vector<TriangleVertexArray*> mTriangleArrays;
List<TriangleVertexArray*> mTriangleArrays;
public:
@ -54,7 +57,7 @@ class TriangleMesh {
TriangleMesh();
/// Destructor
~TriangleMesh();
~TriangleMesh() = default;
/// Add a subpart of the mesh
void addSubpart(TriangleVertexArray* triangleVertexArray);
@ -67,17 +70,27 @@ class TriangleMesh {
};
// Add a subpart of the mesh
/**
* @param triangleVertexArray Pointer to the TriangleVertexArray to add into the mesh
*/
inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) {
mTriangleArrays.push_back(triangleVertexArray );
mTriangleArrays.add(triangleVertexArray );
}
// Return a pointer to a given subpart (triangle vertex array) of the mesh
/**
* @param indexSubpart The index of the sub-part of the mesh
* @return A pointer to the triangle vertex array of a given sub-part of the mesh
*/
inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const {
assert(indexSubpart < mTriangleArrays.size());
return mTriangleArrays[indexSubpart];
}
// Return the number of subparts of the mesh
// Return the number of sub-parts of the mesh
/**
* @return The number of sub-parts of the mesh
*/
inline uint TriangleMesh::getNbSubparts() const {
return mTriangleArrays.size();
}

309
src/collision/TriangleVertexArray.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -25,37 +25,330 @@
// Libraries
#include "TriangleVertexArray.h"
#include "mathematics/Vector3.h"
#include <cassert>
using namespace reactphysics3d;
// Constructor
// Constructor without vertices normals
/// Note that your data will not be copied into the TriangleVertexArray and
/// therefore, you need to make sure that those data are always valid during
/// the lifetime of the TriangleVertexArray.
/// the lifetime of the TriangleVertexArray. With this constructor, you do not
/// need to provide vertices normals for smooth mesh collision. Therefore, the
/// vertices normals will be computed automatically. The vertices normals are
/// computed with weighted average of the associated triangle face normal. The
/// weights are the angle between the associated edges of neighbor triangle face.
/**
* @param nbVertices Number of vertices in the array
* @param verticesStart Pointer to the first vertices of the array
* @param verticesStride Number of bytes between the beginning of two consecutive vertices
* @param nbTriangles Number of triangles in the array
* @param indexesStart Pointer to the first triangle index
* @param indexesStride Number of bytes between the beginning of the three indices of two triangles
* @param vertexDataType Type of data for the vertices (float, double)
* @param indexDataType Type of data for the indices (short, int)
*/
TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) {
mNbVertices = nbVertices;
mVerticesStart = static_cast<const uchar*>(verticesStart);
mVerticesStride = verticesStride;
mVerticesNormalsStart = nullptr;
mVerticesNormalsStride = 3 * sizeof(float);
mNbTriangles = nbTriangles;
mIndicesStart = static_cast<const uchar*>(indexesStart);
mIndicesStride = indexesStride;
mVertexDataType = vertexDataType;
mVertexNormaldDataType = NormalDataType::NORMAL_FLOAT_TYPE;
mIndexDataType = indexDataType;
mAreVerticesNormalsProvidedByUser = false;
// Compute the vertices normals because they are not provided by the user
computeVerticesNormals();
}
// Constructor with vertices normals
/// Note that your data will not be copied into the TriangleVertexArray and
/// therefore, you need to make sure that those data are always valid during
/// the lifetime of the TriangleVertexArray. With this constructor, you need
/// to provide the vertices normals that will be used for smooth mesh collision.
/**
* @param nbVertices Number of vertices in the array
* @param verticesStart Pointer to the first vertices of the array
* @param verticesStride Number of bytes between the beginning of two consecutive vertices
* @param verticesNormalsStart Pointer to the first vertex normal of the array
* @param verticesNormalsStride Number of bytes between the beginning of two consecutive vertex normals
* @param nbTriangles Number of triangles in the array
* @param indexesStart Pointer to the first triangle index
* @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
* @param vertexDataType Type of data for the vertices (float, double)
* @param indexDataType Type of data for the indices (short, int)
*/
TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) {
TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
const void* verticesNormalsStart, uint verticesNormalsStride,
uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, NormalDataType normalDataType,
IndexDataType indexDataType) {
mNbVertices = nbVertices;
mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
mVerticesStart = static_cast<const uchar*>(verticesStart);
mVerticesStride = verticesStride;
mVerticesNormalsStart = static_cast<const uchar*>(verticesNormalsStart);
mVerticesNormalsStride = verticesNormalsStride;
mNbTriangles = nbTriangles;
mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
mIndicesStart = static_cast<const uchar*>(indexesStart);
mIndicesStride = indexesStride;
mVertexDataType = vertexDataType;
mVertexNormaldDataType = normalDataType;
mIndexDataType = indexDataType;
mAreVerticesNormalsProvidedByUser = true;
assert(mVerticesNormalsStart != nullptr);
}
// Destructor
TriangleVertexArray::~TriangleVertexArray() {
// If the vertices normals have not been provided by the user
if (!mAreVerticesNormalsProvidedByUser) {
// Release the allocated memory
const void* verticesNormalPointer = static_cast<const void*>(mVerticesNormalsStart);
const float* verticesNormals = static_cast<const float*>(verticesNormalPointer);
delete[] verticesNormals;
}
}
// Compute the vertices normals when they are not provided by the user
/// The vertices normals are computed with weighted average of the associated
/// triangle face normal. The weights are the angle between the associated edges
/// of neighbor triangle face.
void TriangleVertexArray::computeVerticesNormals() {
// Allocate memory for the vertices normals
float* verticesNormals = new float[mNbVertices * 3];
// Init vertices normals to zero
for (uint i=0; i<mNbVertices * 3; i++) {
verticesNormals[i] = 0.0f;
}
// For each triangle face in the array
for (uint f=0; f < mNbTriangles; f++) {
// Get the indices of the three vertices of the triangle in the array
uint verticesIndices[3];
getTriangleVerticesIndices(f, verticesIndices);
// Get the triangle vertices
Vector3 triangleVertices[3];
getTriangleVertices(f, triangleVertices);
// Edges lengths
decimal edgesLengths[3];
edgesLengths[0] = (triangleVertices[1] - triangleVertices[0]).length();
edgesLengths[1] = (triangleVertices[2] - triangleVertices[1]).length();
edgesLengths[2] = (triangleVertices[0] - triangleVertices[2]).length();
// For each vertex of the face
for (uint v=0; v < 3; v++) {
uint previousVertex = (v == 0) ? 2 : v-1;
uint nextVertex = (v == 2) ? 0 : v+1;
Vector3 a = triangleVertices[nextVertex] - triangleVertices[v];
Vector3 b = triangleVertices[previousVertex] - triangleVertices[v];
Vector3 crossProduct = a.cross(b);
decimal sinA = crossProduct.length() / (edgesLengths[previousVertex] * edgesLengths[v]);
sinA = std::min(std::max(sinA, decimal(0.0)), decimal(1.0));
decimal arcSinA = std::asin(sinA);
assert(arcSinA >= decimal(0.0));
Vector3 normalComponent = arcSinA * crossProduct;
// Add the normal component of this vertex into the normals array
verticesNormals[verticesIndices[v] * 3] += normalComponent.x;
verticesNormals[verticesIndices[v] * 3 + 1] += normalComponent.y;
verticesNormals[verticesIndices[v] * 3 + 2] += normalComponent.z;
}
}
// Normalize the computed vertices normals
for (uint v=0; v<mNbVertices * 3; v += 3) {
// Normalize the normal
Vector3 normal(verticesNormals[v], verticesNormals[v + 1], verticesNormals[v + 2]);
normal.normalize();
verticesNormals[v] = normal.x;
verticesNormals[v + 1] = normal.y;
verticesNormals[v + 2] = normal.z;
}
const void* verticesNormalsPointer = static_cast<const void*>(verticesNormals);
mVerticesNormalsStart = static_cast<const uchar*>(verticesNormalsPointer);
}
// Return the indices of the three vertices of a given triangle in the array
/**
* @param triangleIndex Index of a given triangle in the array
* @param[out] outVerticesIndices Pointer to the three output vertex indices
*/
void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const {
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * mIndicesStride;
const void* startTriangleIndices = static_cast<const void*>(triangleIndicesPointer);
// For each vertex of the triangle
for (int i=0; i < 3; i++) {
// Get the index of the current vertex in the triangle
if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
outVerticesIndices[i] = static_cast<const uint*>(startTriangleIndices)[i];
}
else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
outVerticesIndices[i] = static_cast<const ushort*>(startTriangleIndices)[i];
}
else {
assert(false);
}
}
}
// Return the three vertices coordinates of a triangle
/**
* @param triangleIndex Index of a given triangle in the array
* @param[out] outTriangleVertices Pointer to the three output vertex coordinates
*/
void TriangleVertexArray::getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const {
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
// Get the three vertex index of the three vertices of the triangle
uint verticesIndices[3];
getTriangleVerticesIndices(triangleIndex, verticesIndices);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
const uchar* vertexPointerChar = mVerticesStart + verticesIndices[k] * mVerticesStride;
const void* vertexPointer = static_cast<const void*>(vertexPointerChar);
// Get the vertices components of the triangle
if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
const float* vertices = static_cast<const float*>(vertexPointer);
outTriangleVertices[k][0] = decimal(vertices[0]);
outTriangleVertices[k][1] = decimal(vertices[1]);
outTriangleVertices[k][2] = decimal(vertices[2]);
}
else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
const double* vertices = static_cast<const double*>(vertexPointer);
outTriangleVertices[k][0] = decimal(vertices[0]);
outTriangleVertices[k][1] = decimal(vertices[1]);
outTriangleVertices[k][2] = decimal(vertices[2]);
}
else {
assert(false);
}
}
}
// Return the three vertices normals of a triangle
/**
* @param triangleIndex Index of a given triangle in the array
* @param[out] outTriangleVerticesNormals Pointer to the three output vertex normals
*/
void TriangleVertexArray::getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const {
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
// Get the three vertex index of the three vertices of the triangle
uint verticesIndices[3];
getTriangleVerticesIndices(triangleIndex, verticesIndices);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
const uchar* vertexNormalPointerChar = mVerticesNormalsStart + verticesIndices[k] * mVerticesNormalsStride;
const void* vertexNormalPointer = static_cast<const void*>(vertexNormalPointerChar);
// Get the normals from the array
if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) {
const float* normal = static_cast<const float*>(vertexNormalPointer);
outTriangleVerticesNormals[k][0] = decimal(normal[0]);
outTriangleVerticesNormals[k][1] = decimal(normal[1]);
outTriangleVerticesNormals[k][2] = decimal(normal[2]);
}
else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) {
const double* normal = static_cast<const double*>(vertexNormalPointer);
outTriangleVerticesNormals[k][0] = decimal(normal[0]);
outTriangleVerticesNormals[k][1] = decimal(normal[1]);
outTriangleVerticesNormals[k][2] = decimal(normal[2]);
}
else {
assert(false);
}
}
}
// Return a vertex of the array
/**
* @param vertexIndex Index of a given vertex of the array
* @param[out] outVertex Pointer to the output vertex coordinates
*/
void TriangleVertexArray::getVertex(uint vertexIndex, Vector3* outVertex) {
assert(vertexIndex < mNbVertices);
const uchar* vertexPointerChar = mVerticesStart + vertexIndex * mVerticesStride;
const void* vertexPointer = static_cast<const void*>(vertexPointerChar);
// Get the vertices components of the triangle
if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
const float* vertices = static_cast<const float*>(vertexPointer);
(*outVertex)[0] = decimal(vertices[0]);
(*outVertex)[1] = decimal(vertices[1]);
(*outVertex)[2] = decimal(vertices[2]);
}
else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
const double* vertices = static_cast<const double*>(vertexPointer);
(*outVertex)[0] = decimal(vertices[0]);
(*outVertex)[1] = decimal(vertices[1]);
(*outVertex)[2] = decimal(vertices[2]);
}
else {
assert(false);
}
}
// Return a vertex normal of the array
/**
* @param vertexIndex Index of a given vertex of the array
* @param[out] outNormal Pointer to the output vertex normal
*/
void TriangleVertexArray::getNormal(uint vertexIndex, Vector3* outNormal) {
assert(vertexIndex < mNbVertices);
const uchar* vertexNormalPointerChar = mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride;
const void* vertexNormalPointer = static_cast<const void*>(vertexNormalPointerChar);
// Get the normals from the array
if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) {
const float* normal = static_cast<const float*>(vertexNormalPointer);
(*outNormal)[0] = decimal(normal[0]);
(*outNormal)[1] = decimal(normal[1]);
(*outNormal)[2] = decimal(normal[2]);
}
else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) {
const double* normal = static_cast<const double*>(vertexNormalPointer);
(*outNormal)[0] = decimal(normal[0]);
(*outNormal)[1] = decimal(normal[1]);
(*outNormal)[2] = decimal(normal[2]);
}
else {
assert(false);
}
}

152
src/collision/TriangleVertexArray.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -31,6 +31,9 @@
namespace reactphysics3d {
// Declarations
struct Vector3;
// Class TriangleVertexArray
/**
* This class is used to describe the vertices and faces of a triangular mesh.
@ -46,52 +49,92 @@ class TriangleVertexArray {
public:
/// Data type for the vertices in the array
enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
/// Data type for the vertex normals in the array
enum class NormalDataType {NORMAL_FLOAT_TYPE, NORMAL_DOUBLE_TYPE};
/// Data type for the indices in the array
enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
protected:
// -------------------- Attributes -------------------- //
/// Number of vertices in the array
uint mNbVertices;
/// Pointer to the first vertex value in the array
unsigned char* mVerticesStart;
const uchar* mVerticesStart;
/// Stride (number of bytes) between the beginning of two vertices
/// values in the array
int mVerticesStride;
uint mVerticesStride;
/// Pointer to the first vertex normal value in the array
const uchar* mVerticesNormalsStart;
/// Stride (number of bytes) between the beginning of two vertex normals
/// values in the array
uint mVerticesNormalsStride;
/// Number of triangles in the array
uint mNbTriangles;
/// Pointer to the first vertex index of the array
unsigned char* mIndicesStart;
const uchar* mIndicesStart;
/// Stride (number of bytes) between the beginning of two indices in
/// the array
int mIndicesStride;
/// Stride (number of bytes) between the beginning of the three indices of two triangles
uint mIndicesStride;
/// Data type of the vertices in the array
VertexDataType mVertexDataType;
/// Data type of the vertex normals in the array
NormalDataType mVertexNormaldDataType;
/// Data type of the indices in the array
IndexDataType mIndexDataType;
/// True if the vertices normals are provided by the user
bool mAreVerticesNormalsProvidedByUser;
// -------------------- Methods -------------------- //
/// Compute the vertices normals when they are not provided by the user
void computeVerticesNormals();
public:
/// Constructor
TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride,
// -------------------- Methods -------------------- //
/// Constructor without vertices normals
TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType);
/// Constructor with vertices normals
TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
const void* verticesNormalsStart, uint uverticesNormalsStride,
uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, NormalDataType normalDataType,
IndexDataType indexDataType);
/// Destructor
~TriangleVertexArray();
/// Deleted assignment operator
TriangleVertexArray& operator=(const TriangleVertexArray& triangleVertexArray) = delete;
/// Deleted copy-constructor
TriangleVertexArray(const TriangleVertexArray& triangleVertexArray) = delete;
/// Return the vertex data type
VertexDataType getVertexDataType() const;
/// Return the vertex normal data type
NormalDataType getVertexNormalDataType() const;
/// Return the index data type
IndexDataType getIndexDataType() const;
@ -102,55 +145,124 @@ class TriangleVertexArray {
uint getNbTriangles() const;
/// Return the vertices stride (number of bytes)
int getVerticesStride() const;
uint getVerticesStride() const;
/// Return the vertex normals stride (number of bytes)
uint getVerticesNormalsStride() const;
/// Return the indices stride (number of bytes)
int getIndicesStride() const;
uint getIndicesStride() const;
/// Return the pointer to the start of the vertices array
unsigned char* getVerticesStart() const;
const void* getVerticesStart() const;
/// Return the pointer to the start of the vertex normals array
const void* getVerticesNormalsStart() const;
/// Return the pointer to the start of the indices array
unsigned char* getIndicesStart() const;
const void* getIndicesStart() const;
/// Return the vertices coordinates of a triangle
void getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const;
/// Return the three vertices normals of a triangle
void getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const;
/// Return the indices of the three vertices of a given triangle in the array
void getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const;
/// Return a vertex of the array
void getVertex(uint vertexIndex, Vector3* outVertex);
/// Return a vertex normal of the array
void getNormal(uint vertexIndex, Vector3* outNormal);
};
// Return the vertex data type
/**
* @return The data type of the vertices in the array
*/
inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const {
return mVertexDataType;
}
// Return the vertex normal data type
/**
* @return The data type of the normals in the array
*/
inline TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalDataType() const {
return mVertexNormaldDataType;
}
// Return the index data type
/**
* @return The data type of the face indices in the array
*/
inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const {
return mIndexDataType;
}
// Return the number of vertices
/**
* @return The number of vertices in the array
*/
inline uint TriangleVertexArray::getNbVertices() const {
return mNbVertices;
}
// Return the number of triangles
/**
* @return The number of triangles in the array
*/
inline uint TriangleVertexArray::getNbTriangles() const {
return mNbTriangles;
}
// Return the vertices stride (number of bytes)
inline int TriangleVertexArray::getVerticesStride() const {
/**
* @return The number of bytes separating two consecutive vertices in the array
*/
inline uint TriangleVertexArray::getVerticesStride() const {
return mVerticesStride;
}
// Return the vertex normals stride (number of bytes)
/**
* @return The number of bytes separating two consecutive normals in the array
*/
inline uint TriangleVertexArray::getVerticesNormalsStride() const {
return mVerticesNormalsStride;
}
// Return the indices stride (number of bytes)
inline int TriangleVertexArray::getIndicesStride() const {
/**
* @return The number of bytes separating two consecutive face indices in the array
*/
inline uint TriangleVertexArray::getIndicesStride() const {
return mIndicesStride;
}
// Return the pointer to the start of the vertices array
inline unsigned char* TriangleVertexArray::getVerticesStart() const {
/**
* @return A pointer to the start of the vertices data in the array
*/
inline const void* TriangleVertexArray::getVerticesStart() const {
return mVerticesStart;
}
// Return the pointer to the start of the vertex normals array
/**
* @return A pointer to the start of the normals data in the array
*/
inline const void* TriangleVertexArray::getVerticesNormalsStart() const {
return mVerticesNormalsStart;
}
// Return the pointer to the start of the indices array
inline unsigned char* TriangleVertexArray::getIndicesStart() const {
/**
* @return A pointer to the start of the face indices data in the array
*/
inline const void* TriangleVertexArray::getIndicesStart() const {
return mIndicesStart;
}

182
src/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -26,57 +26,103 @@
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "collision/CollisionDetection.h"
#include "engine/Profiler.h"
#include "utils/Profiler.h"
#include "collision/RaycastInfo.h"
#include "memory/MemoryManager.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:mDynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
:mDynamicAABBTree(collisionDetection.getMemoryManager().getPoolAllocator(), DYNAMIC_TREE_AABB_GAP),
mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
mCollisionDetection(collisionDetection) {
PoolAllocator& poolAllocator = collisionDetection.getMemoryManager().getPoolAllocator();
// Allocate memory for the array of non-static proxy shapes IDs
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
assert(mMovedShapes != NULL);
mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
assert(mMovedShapes != nullptr);
// Allocate memory for the array of potential overlapping pairs
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs != NULL);
mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
assert(mPotentialPairs != nullptr);
#ifdef IS_PROFILING_ACTIVE
mProfiler = nullptr;
#endif
}
// Destructor
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// Get the memory pool allocatory
PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
// Release the memory for the array of non-static proxy shapes IDs
free(mMovedShapes);
poolAllocator.release(mMovedShapes, mNbAllocatedMovedShapes * sizeof (int));
// Release the memory for the array of potential overlapping pairs
free(mPotentialPairs);
poolAllocator.release(mPotentialPairs, mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
}
// Add a collision shape in the array of shapes that have moved in the last simulation step
// and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {
// Allocate more elements in the array of shapes that have moved if necessary
if (mNbAllocatedMovedShapes == mNbMovedShapes) {
// Get the memory pool allocatory
PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
uint oldNbAllocatedMovedShapes = mNbAllocatedMovedShapes;
mNbAllocatedMovedShapes *= 2;
int* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
assert(mMovedShapes != NULL);
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
free(oldArray);
mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
assert(mMovedShapes != nullptr);
std::memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
poolAllocator.release(oldArray, oldNbAllocatedMovedShapes * sizeof(int));
}
// Store the broad-phase ID into the array of shapes that have moved
assert(mNbMovedShapes < mNbAllocatedMovedShapes);
assert(mMovedShapes != NULL);
assert(mMovedShapes != nullptr);
mMovedShapes[mNbMovedShapes] = broadPhaseID;
mNbMovedShapes++;
}
// Return true if the two broad-phase collision shapes are overlapping
bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
const ProxyShape* shape2) const {
if (shape1->getBroadPhaseId() == -1 || shape2->getBroadPhaseId() == -1) return false;
// Get the two AABBs of the collision shapes
const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->getBroadPhaseId());
const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->getBroadPhaseId());
// Check if the two AABBs are overlapping
return aabb1.testCollision(aabb2);
}
// Ray casting method
void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
RP3D_PROFILE("BroadPhaseAlgorithm::raycast()", mProfiler);
BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
}
// Remove a collision shape from the array of shapes that have moved in the last simulation step
// and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
@ -88,10 +134,14 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
mNbAllocatedMovedShapes > 8) {
// Get the memory pool allocatory
PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
uint oldNbAllocatedMovedShapes = mNbAllocatedMovedShapes;
mNbAllocatedMovedShapes /= 2;
int* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
assert(mMovedShapes != NULL);
mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
assert(mMovedShapes != nullptr);
uint nbElements = 0;
for (uint i=0; i<mNbMovedShapes; i++) {
if (oldArray[i] != -1) {
@ -101,7 +151,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
}
mNbMovedShapes = nbElements;
mNbNonUsedMovedShapes = 0;
free(oldArray);
poolAllocator.release(oldArray, oldNbAllocatedMovedShapes * sizeof(int));
}
// Remove the broad-phase ID from the array
@ -117,6 +167,8 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
// Add a proxy collision shape into the broad-phase collision detection
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
assert(proxyShape->getBroadPhaseId() == -1);
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape);
@ -125,13 +177,17 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const A
// Add the collision shape into the array of bodies that have moved (or have been created)
// during the last simulation step
addMovedCollisionShape(proxyShape->mBroadPhaseID);
addMovedCollisionShape(proxyShape->getBroadPhaseId());
}
// Remove a proxy collision shape from the broad-phase collision detection
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
int broadPhaseID = proxyShape->mBroadPhaseID;
assert(proxyShape->getBroadPhaseId() != -1);
int broadPhaseID = proxyShape->getBroadPhaseId();
proxyShape->mBroadPhaseID = -1;
// Remove the collision shape from the dynamic AABB tree
mDynamicAABBTree.removeObject(broadPhaseID);
@ -145,7 +201,7 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert) {
int broadPhaseID = proxyShape->mBroadPhaseID;
int broadPhaseID = proxyShape->getBroadPhaseId();
assert(broadPhaseID >= 0);
@ -162,12 +218,25 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons
}
}
void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb,
LinkedList<int>& overlappingNodes) const {
AABBOverlapCallback callback(overlappingNodes);
// Ask the dynamic AABB tree to report all collision shapes that overlap with this AABB
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(aabb, callback);
}
// Compute all the overlapping pairs of collision shapes
void BroadPhaseAlgorithm::computeOverlappingPairs() {
void BroadPhaseAlgorithm::computeOverlappingPairs(MemoryManager& memoryManager) {
// TODO : Try to see if we can allocate potential pairs in single frame allocator
// Reset the potential overlapping pairs
mNbPotentialPairs = 0;
LinkedList<int> overlappingNodes(memoryManager.getPoolAllocator());
// For all collision shapes that have moved (or have been created) during the
// last simulation step
for (uint i=0; i<mNbMovedShapes; i++) {
@ -175,7 +244,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
if (shapeID == -1) continue;
AABBOverlapCallback callback(*this, shapeID);
AABBOverlapCallback callback(overlappingNodes);
// Get the AABB of the shape
const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
@ -184,6 +253,12 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair.
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
// Add the potential overlapping pairs
addOverlappingNodes(shapeID, overlappingNodes);
// Remove all the elements of the linked list of overlapping nodes
overlappingNodes.reset();
}
// Reset the array of collision shapes that have move (or have been created) during the
@ -208,8 +283,12 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
// Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
// If the two proxy collision shapes are from the same body, skip it
if (shape1->getBody()->getId() != shape2->getBody()->getId()) {
// Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
}
// Skip the duplicate overlapping pairs
while (i < mNbPotentialPairs) {
@ -230,45 +309,58 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// number of overlapping pairs
if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) {
PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
// Reduce the number of allocated potential overlapping pairs
BroadPhasePair* oldPairs = mPotentialPairs;
uint oldNbAllocatedPotentialPairs = mNbAllocatedPotentialPairs;
mNbAllocatedPotentialPairs /= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
poolAllocator.release(oldPairs, oldNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
}
}
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
void BroadPhaseAlgorithm::addOverlappingNodes(int referenceNodeId, const LinkedList<int>& overlappingNodes) {
// If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return;
// For each overlapping node in the linked list
LinkedList<int>::ListElement* elem = overlappingNodes.getListHead();
while (elem != nullptr) {
// If we need to allocate more memory for the array of potential overlapping pairs
if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
// If both the nodes are the same, we do not create store the overlapping pair
if (referenceNodeId != elem->data) {
// Allocate more memory for the array of potential pairs
BroadPhasePair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs *= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
// If we need to allocate more memory for the array of potential overlapping pairs
if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
// Allocate more memory for the array of potential pairs
BroadPhasePair* oldPairs = mPotentialPairs;
uint oldNbAllocatedPotentialPairs = mNbAllocatedPotentialPairs;
mNbAllocatedPotentialPairs *= 2;
mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
poolAllocator.release(oldPairs, oldNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
}
// Add the new potential pair into the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(referenceNodeId, elem->data);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(referenceNodeId, elem->data);
mNbPotentialPairs++;
}
elem = elem->next;
}
// Add the new potential pair into the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++;
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
mOverlappingNodes.insert(nodeId);
}
// Called for a broad-phase shape that has to be tested for raycast

105
src/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,11 +27,8 @@
#define REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H
// Libraries
#include <vector>
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "DynamicAABBTree.h"
#include "engine/Profiler.h"
#include "containers/LinkedList.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -39,6 +36,10 @@ namespace reactphysics3d {
// Declarations
class CollisionDetection;
class BroadPhaseAlgorithm;
class CollisionBody;
class ProxyShape;
class MemoryManager;
class Profiler;
// Structure BroadPhasePair
/**
@ -66,21 +67,19 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int mReferenceNodeId;
public:
LinkedList<int>& mOverlappingNodes;
// Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId)
: mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
AABBOverlapCallback(LinkedList<int>& overlappingNodes)
: mOverlappingNodes(overlappingNodes) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId);
virtual void notifyOverlappingNode(int nodeId) override;
};
@ -109,8 +108,11 @@ class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
}
// Destructor
virtual ~BroadPhaseRaycastCallback() override = default;
// Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray);
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray) override;
};
@ -159,14 +161,13 @@ class BroadPhaseAlgorithm {
/// Reference to the collision detection object
CollisionDetection& mCollisionDetection;
// -------------------- Methods -------------------- //
/// Private copy-constructor
BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm);
#ifdef IS_PROFILING_ACTIVE
/// Private assignment operator
BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm);
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public :
@ -177,6 +178,12 @@ class BroadPhaseAlgorithm {
/// Destructor
~BroadPhaseAlgorithm();
/// Deleted copy-constructor
BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm) = delete;
/// Deleted assignment operator
BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm) = delete;
/// Add a proxy collision shape into the broad-phase collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
@ -196,18 +203,34 @@ class BroadPhaseAlgorithm {
/// step and that need to be tested again for broad-phase overlapping.
void removeMovedCollisionShape(int broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
/// Add potential overlapping pairs in the dynamic AABB tree
void addOverlappingNodes(int broadPhaseId1, const LinkedList<int>& overlappingNodes);
/// Report all the shapes that are overlapping with a given AABB
void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList<int>& overlappingNodes) const;
/// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs();
void computeOverlappingPairs(MemoryManager& memoryManager);
/// Return the proxy shape corresponding to the broad-phase node id in parameter
ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const;
/// Return true if the two broad-phase collision shapes are overlapping
bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const;
/// Return the fat AABB of a given broad-phase shape
const AABB& getFatAABB(int broadPhaseId) const;
/// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const;
void raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
// Method used to compare two pairs for sorting algorithm
@ -220,28 +243,26 @@ inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const Broad
return false;
}
// Return true if the two broad-phase collision shapes are overlapping
inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
const ProxyShape* shape2) const {
// Get the two AABBs of the collision shapes
const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);
// Check if the two AABBs are overlapping
return aabb1.testCollision(aabb2);
// Return the fat AABB of a given broad-phase shape
inline const AABB& BroadPhaseAlgorithm::getFatAABB(int broadPhaseId) const {
return mDynamicAABBTree.getFatAABB(broadPhaseId);
}
// Ray casting method
inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
PROFILE("BroadPhaseAlgorithm::raycast()");
BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
// Return the proxy shape corresponding to the broad-phase node id in parameter
inline ProxyShape* BroadPhaseAlgorithm::getProxyShapeForBroadPhaseId(int broadPhaseId) const {
return static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(broadPhaseId));
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void BroadPhaseAlgorithm::setProfiler(Profiler* profiler) {
mProfiler = profiler;
mDynamicAABBTree.setProfiler(profiler);
}
#endif
}
#endif

30
src/collision/broadphase/DynamicAABBTree.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -26,8 +26,8 @@
// Libraries
#include "DynamicAABBTree.h"
#include "BroadPhaseAlgorithm.h"
#include "memory/Stack.h"
#include "engine/Profiler.h"
#include "containers/Stack.h"
#include "utils/Profiler.h"
using namespace reactphysics3d;
@ -35,7 +35,8 @@ using namespace reactphysics3d;
const int TreeNode::NULL_TREE_NODE = -1;
// Constructor
DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) {
DynamicAABBTree::DynamicAABBTree(MemoryAllocator& allocator, decimal extraAABBGap)
: mAllocator(allocator), mExtraAABBGap(extraAABBGap) {
init();
}
@ -44,7 +45,7 @@ DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABB
DynamicAABBTree::~DynamicAABBTree() {
// Free the allocated memory for the nodes
free(mNodes);
mAllocator.release(mNodes, mNbAllocatedNodes * sizeof(TreeNode));
}
// Initialize the tree
@ -55,9 +56,9 @@ void DynamicAABBTree::init() {
mNbAllocatedNodes = 8;
// Allocate memory for the nodes of the tree
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
mNodes = static_cast<TreeNode*>(mAllocator.allocate(mNbAllocatedNodes * sizeof(TreeNode)));
assert(mNodes);
memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
std::memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
// Initialize the allocated nodes
for (int i=0; i<mNbAllocatedNodes - 1; i++) {
@ -73,7 +74,7 @@ void DynamicAABBTree::init() {
void DynamicAABBTree::reset() {
// Free the allocated memory for the nodes
free(mNodes);
mAllocator.release(mNodes, mNbAllocatedNodes * sizeof(TreeNode));
// Initialize the tree
init();
@ -88,12 +89,13 @@ int DynamicAABBTree::allocateNode() {
assert(mNbNodes == mNbAllocatedNodes);
// Allocate more nodes in the tree
uint oldNbAllocatedNodes = mNbAllocatedNodes;
mNbAllocatedNodes *= 2;
TreeNode* oldNodes = mNodes;
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
mNodes = static_cast<TreeNode*>(mAllocator.allocate(mNbAllocatedNodes * sizeof(TreeNode)));
assert(mNodes);
memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
free(oldNodes);
mAllocator.release(oldNodes, oldNbAllocatedNodes * sizeof(TreeNode));
// Initialize the allocated nodes
for (int i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
@ -171,7 +173,7 @@ void DynamicAABBTree::removeObject(int nodeID) {
/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) {
PROFILE("DynamicAABBTree::updateObject()");
RP3D_PROFILE("DynamicAABBTree::updateObject()", mProfiler);
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
@ -596,7 +598,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const {
// Create a stack with the nodes to visit
Stack<int, 64> stack;
Stack<int, 64> stack(mAllocator);
stack.push(mRootNodeID);
// While there are still nodes to visit
@ -633,11 +635,11 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
// Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const {
PROFILE("DynamicAABBTree::raycast()");
RP3D_PROFILE("DynamicAABBTree::raycast()", mProfiler);
decimal maxFraction = ray.maxFraction;
Stack<int, 128> stack;
Stack<int, 128> stack(mAllocator);
stack.push(mRootNodeID);
// Walk through the tree from the root looking for proxy shapes

42
src/collision/broadphase/DynamicAABBTree.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -29,7 +29,6 @@
// Libraries
#include "configuration.h"
#include "collision/shapes/AABB.h"
#include "body/CollisionBody.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -38,7 +37,12 @@ namespace reactphysics3d {
class BroadPhaseAlgorithm;
class BroadPhaseRaycastTestCallback;
class DynamicAABBTreeOverlapCallback;
class CollisionBody;
struct RaycastTest;
class AABB;
class Profiler;
class MemoryAllocator;
// Structure TreeNode
/**
@ -101,6 +105,9 @@ class DynamicAABBTreeOverlapCallback {
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId)=0;
// Destructor
virtual ~DynamicAABBTreeOverlapCallback() = default;
};
// Class DynamicAABBTreeRaycastCallback
@ -115,6 +122,8 @@ class DynamicAABBTreeRaycastCallback {
// Called when the AABB of a leaf node is hit by a ray
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray)=0;
virtual ~DynamicAABBTreeRaycastCallback() = default;
};
// Class DynamicAABBTree
@ -131,6 +140,9 @@ class DynamicAABBTree {
// -------------------- Attributes -------------------- //
/// Memory allocator
MemoryAllocator& mAllocator;
/// Pointer to the memory location of the nodes of the tree
TreeNode* mNodes;
@ -150,6 +162,13 @@ class DynamicAABBTree {
/// without triggering a large modification of the tree which can be costly
decimal mExtraAABBGap;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree
@ -191,7 +210,7 @@ class DynamicAABBTree {
// -------------------- Methods -------------------- //
/// Constructor
DynamicAABBTree(decimal extraAABBGap = decimal(0.0));
DynamicAABBTree(MemoryAllocator& allocator, decimal extraAABBGap = decimal(0.0));
/// Destructor
~DynamicAABBTree();
@ -232,6 +251,14 @@ class DynamicAABBTree {
/// Clear all the nodes and reset the tree
void reset();
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
// Return true if the node is a leaf of the tree
@ -287,6 +314,15 @@ inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) {
return nodeId;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void DynamicAABBTree::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

228
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp Executable file
View File

@ -0,0 +1,228 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "CapsuleVsCapsuleAlgorithm.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/NarrowPhaseInfo.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two capsules
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
// Get the capsule collision shapes
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
// Get the transform from capsule 1 local-space to capsule 2 local-space
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
// Compute the end-points of the inner segment of the first capsule
Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0);
capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA;
capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB;
// Compute the end-points of the inner segment of the second capsule
const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0);
const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0);
// The two inner capsule segments
const Vector3 seg1 = capsule1SegB - capsule1SegA;
const Vector3 seg2 = capsule2SegB - capsule2SegA;
// Compute the sum of the radius of the two capsules (virtual spheres)
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// If the two capsules are parallel (we create two contact points)
bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2);
if (areCapsuleInnerSegmentsParralel) {
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsPerpendicularDistance >= sumRadius) {
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
return false;
}
// Compute the planes that goes through the extreme points of the inner segment of capsule 1
decimal d1 = seg1.dot(capsule1SegA);
decimal d2 = -seg1.dot(capsule1SegB);
// Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
// segment of capsule 1
decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
// If the segments were overlapping (the clip segment is valid)
if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
if (reportContacts) {
// Clip the inner segment of capsule 2
if (t1 > decimal(1.0)) t1 = decimal(1.0);
const Vector3 clipPointA = capsule2SegB - t1 * seg2;
if (t2 > decimal(1.0)) t2 = decimal(1.0);
const Vector3 clipPointB = capsule2SegA + t2 * seg2;
// Project point capsule2SegA onto line of innner segment of capsule 1
const Vector3 seg1Normalized = seg1.getUnit();
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
Vector3 normalCapsule2SpaceNormalized;
Vector3 segment1ToSegment2;
// If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping)
if (segmentsPerpendicularDistance > MACHINE_EPSILON) {
// Compute a perpendicular vector from segment 1 to segment 2
segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit();
}
else { // If the capsule inner segments are overlapping (degenerate case)
// We cannot use the vector between segments as a contact normal. To generate a contact normal, we take
// any vector that is orthogonal to the inner capsule segments.
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
Vector3 seg2Normalized = seg2.getUnit();
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2))
segment1ToSegment2.setToZero();
// We choose as a contact normal, any direction that is perpendicular to the inner capsules segments
normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2);
}
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
}
return true;
}
}
// Compute the closest points between the two inner capsule segments
Vector3 closestPointCapsule1Seg;
Vector3 closestPointCapsule2Seg;
computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
closestPointCapsule1Seg, closestPointCapsule2Seg);
// Compute the distance between the sphere center and the closest point on the segment
Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap
if (closestPointsDistanceSquare < sumRadius * sumRadius) {
if (reportContacts) {
// If the distance between the inner segments is not zero
if (closestPointsDistanceSquare > MACHINE_EPSILON) {
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
closestPointsSeg1ToSeg2 /= closestPointsDistance;
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
decimal penetrationDepth = sumRadius - closestPointsDistance;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // The segment are overlapping (degenerate case)
// If the capsule segments are parralel
if (areCapsuleInnerSegmentsParralel) {
// The segment are parallel, not overlapping and their distance is zero.
// Therefore, the capsules are just touching at the top of their inner segments
decimal squareDistCapsule2PointToCapsuleSegA = (capsule1SegA - closestPointCapsule2Seg).lengthSquare();
Vector3 capsule1SegmentMostExtremePoint = squareDistCapsule2PointToCapsuleSegA > MACHINE_EPSILON ? capsule1SegA : capsule1SegB;
Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint);
normalCapsuleSpace2.normalize();
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // If the capsules inner segments are not parallel
// We cannot use a vector between the segments as contact normal. We need to compute a new contact normal with the cross
// product between the two segments.
Vector3 normalCapsuleSpace2 = seg1.cross(seg2);
normalCapsuleSpace2.normalize();
// Compute the contact points on both shapes
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
}
}
return true;
}
return false;
}

74
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h Normal file
View File

@ -0,0 +1,74 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class Body;
class ContactPoint;
// Class CapsuleVsCapsuleAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two capsules collision shapes. We do not use the GJK or SAT
* algorithm here. We directly compute the contact points and contact normal.
* This is based on the "Robust Contact Creation for Physics Simulation"
* presentation by Dirk Gregorius.
*/
class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
CapsuleVsCapsuleAlgorithm() = default;
/// Destructor
virtual ~CapsuleVsCapsuleAlgorithm() override = default;
/// Deleted copy-constructor
CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Deleted assignment operator
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two capsules
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

166
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp Normal file
View File

@ -0,0 +1,166 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "CapsuleVsConvexPolyhedronAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/ConvexPolyhedronShape.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/ContactPointInfo.h"
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a capsule and a polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
gjkAlgorithm.setProfiler(mProfiler);
satAlgorithm.setProfiler(mProfiler);
#endif
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
if (reportContacts) {
// GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the
// capsule inner segment and parallel to the contact point normal, we would like to create
// two contact points instead of a single one (as in the deep contact case with SAT algorithm)
// Get the contact point created by GJK
ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
assert(contactPoint != nullptr);
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
// For each face of the polyhedron
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
capsuleInnerSegmentDirection.normalize();
bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
// If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
// is in direction of the contact normal (from the polyhedron point of view).
if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection)
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// Remove the previous contact point computed by GJK
narrowPhaseInfo->resetContactPoints();
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
// Compute the end-points of the inner segment of the capsule
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
// Convert the inner capsule segment points into the polyhedron local-space
const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
if (isCapsuleShape1) {
faceNormalWorld = -faceNormalWorld;
}
// Compute and create two contact points
bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
if (!contactsFound) {
return false;
}
break;
}
}
}
lastFrameCollisionInfo->wasUsingSAT = false;
lastFrameCollisionInfo->wasUsingGJK = false;
// Return true
return true;
}
// If we have overlap even without the margins (deep penetration)
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
}
return false;
}

79
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h Normal file
View File

@ -0,0 +1,79 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
#define REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class ContactPoint;
class Body;
// Class CapsuleVsConvexPolyhedronAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between a capsule and a convex polyhedron. The capsule is basically
* a line segment with a margin around it. First we run the GJK algorithm.
* If GJK reports separation, we are done. If the objects overlap inside the
* capsule margin (radius), it will also report contact points and normal.
* However, if GJK report penetration of the capsule inner segment within
* the polyhedron, we run the SAT algorithm to get the contact points and
* normal.
* This is based on the "Robust Contact Creation for Physics Simulation"
* presentation by Dirk Gregorius.
*/
class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
CapsuleVsConvexPolyhedronAlgorithm() = default;
/// Destructor
virtual ~CapsuleVsConvexPolyhedronAlgorithm() override = default;
/// Deleted copy-constructor
CapsuleVsConvexPolyhedronAlgorithm(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Deleted assignment operator
CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a capsule and a polyhedron
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

45
src/collision/narrowphase/CollisionDispatch.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,10 +27,13 @@
#define REACTPHYSICS3D_COLLISION_DISPATCH_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
namespace reactphysics3d {
// Declarations
class NarrowPhaseAlgorithm;
class Profiler;
// Class CollisionDispatch
/**
* This is the abstract base class for dispatching the narrow-phase
@ -41,26 +44,44 @@ class CollisionDispatch {
protected:
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public:
/// Constructor
CollisionDispatch() {}
CollisionDispatch() = default;
/// Destructor
virtual ~CollisionDispatch() {}
/// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
}
virtual ~CollisionDispatch() = default;
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type,
int shape2Type)=0;
virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type)=0;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler);
#endif
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void CollisionDispatch::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

291
src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

@ -1,291 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "collision/shapes/ConcaveShape.h"
#include "collision/shapes/TriangleShape.h"
#include "ConcaveVsConvexAlgorithm.h"
#include "collision/CollisionDetection.h"
#include "engine/CollisionWorld.h"
#include <algorithm>
using namespace reactphysics3d;
// Constructor
ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() {
}
// Destructor
ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() {
}
// Return true and compute a contact info if the two bounding volumes collide
void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
ProxyShape* convexProxyShape;
ProxyShape* concaveProxyShape;
const ConvexShape* convexShape;
const ConcaveShape* concaveShape;
// Collision shape 1 is convex, collision shape 2 is concave
if (shape1Info.collisionShape->isConvex()) {
convexProxyShape = shape1Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
concaveProxyShape = shape2Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
}
else { // Collision shape 2 is convex, collision shape 1 is concave
convexProxyShape = shape2Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
concaveProxyShape = shape1Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
}
// Set the parameters of the callback object
ConvexVsTriangleCallback convexVsTriangleCallback;
convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
convexVsTriangleCallback.setConvexShape(convexShape);
convexVsTriangleCallback.setConcaveShape(concaveShape);
convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
// Compute the convex shape AABB in the local-space of the convex shape
AABB aabb;
convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
// If smooth mesh collision is enabled for the concave mesh
if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
std::vector<SmoothMeshContactInfo> contactPoints;
SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
// Run the smooth mesh collision algorithm
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
}
else {
convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
}
}
// Test collision between a triangle and the convex mesh shape
void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape
decimal margin = mConcaveShape->getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// Select the collision algorithm to use between the triangle and the convex shape
NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
mConvexShape->getType());
// If there is no collision algorithm between those two kinds of shapes
if (algo == NULL) return;
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
algo->setCurrentOverlappingPair(mOverlappingPair);
// Create the CollisionShapeInfo objects
CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
mConcaveProxyShape->getLocalToWorldTransform(),
mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
// Use the collision algorithm to test collision between the triangle and the other convex shape
algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
}
// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some internal edges.
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle
std::unordered_multimap<int, Vector3> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth
std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
// For each contact point (from smaller penetration depth to larger)
std::vector<SmoothMeshContactInfo>::const_iterator it;
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
const SmoothMeshContactInfo info = *it;
const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Compute the barycentric coordinates of the point in the triangle
decimal u, v, w;
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
info.triangleVertices[1],
info.triangleVertices[2],
contactPoint, u, v, w);
int nbZeros = 0;
bool isUZero = approxEqual(u, 0, 0.0001);
bool isVZero = approxEqual(v, 0, 0.0001);
bool isWZero = approxEqual(w, 0, 0.0001);
if (isUZero) nbZeros++;
if (isVZero) nbZeros++;
if (isWZero) nbZeros++;
// If it is a vertex contact
if (nbZeros == 2) {
Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
// Check that this triangle vertex has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
}
}
else if (nbZeros == 1) { // If it is an edge contact
Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
// Check that this triangle edge has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
}
}
else { // If it is a face contact
ContactPointInfo newContactInfo(info.contactInfo);
ProxyShape* firstShape;
ProxyShape* secondShape;
if (info.isFirstShapeTriangle) {
firstShape = overlappingPair->getShape1();
secondShape = overlappingPair->getShape2();
}
else {
firstShape = overlappingPair->getShape2();
secondShape = overlappingPair->getShape1();
}
// We use the triangle normal as the contact normal
Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
Vector3 localNormal = a.cross(b);
newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
newContactInfo.normal.normalize();
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
newContactInfo.normal = -newContactInfo.normal;
}
// We recompute the contact point on the second body with the new normal as described in
// the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
// Dirk Gregorius) to avoid adding torque
Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
if (info.isFirstShapeTriangle) {
Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
}
else {
Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
}
// Report the contact
narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
}
// Add the three vertices of the triangle to the set of processed
// triangle vertices
addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
}
}
// Return true if the vertex is in the set of already processed vertices
bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const {
int key = int(vertex.x * vertex.y * vertex.z);
auto range = processTriangleVertices.equal_range(key);
for (auto it = range.first; it != range.second; ++it) {
if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true;
}
return false;
}
// Called by a narrow-phase collision algorithm when a new contact has been found
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) {
Vector3 triangleVertices[3];
bool isFirstShapeTriangle;
// If the collision shape 1 is the triangle
if (contactInfo.collisionShape1->getType() == TRIANGLE) {
assert(contactInfo.collisionShape2->getType() != TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
triangleVertices[0] = triangleShape->getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2);
isFirstShapeTriangle = true;
}
else { // If the collision shape 2 is the triangle
assert(contactInfo.collisionShape2->getType() == TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
triangleVertices[0] = triangleShape->getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2);
isFirstShapeTriangle = false;
}
SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
// Add the narrow-phase contact into the list of contact to process for
// smooth mesh collision
mContactPoints.push_back(smoothContactInfo);
}

234
src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
View File

@ -1,234 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
#include "collision/shapes/ConvexShape.h"
#include "collision/shapes/ConcaveShape.h"
#include <unordered_map>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class ConvexVsTriangleCallback
/**
* This class is used to encapsulate a callback method for
* collision detection between the triangle of a concave mesh shape
* and a convex shape.
*/
class ConvexVsTriangleCallback : public TriangleCallback {
protected:
/// Pointer to the collision detection object
CollisionDetection* mCollisionDetection;
/// Narrow-phase collision callback
NarrowPhaseCallback* mNarrowPhaseCallback;
/// Convex collision shape to test collision with
const ConvexShape* mConvexShape;
/// Concave collision shape
const ConcaveShape* mConcaveShape;
/// Proxy shape of the convex collision shape
ProxyShape* mConvexProxyShape;
/// Proxy shape of the concave collision shape
ProxyShape* mConcaveProxyShape;
/// Broadphase overlapping pair
OverlappingPair* mOverlappingPair;
/// Used to sort ContactPointInfos according to their penetration depth
static bool contactsDepthCompare(const ContactPointInfo& contact1,
const ContactPointInfo& contact2);
public:
/// Set the collision detection pointer
void setCollisionDetection(CollisionDetection* collisionDetection) {
mCollisionDetection = collisionDetection;
}
/// Set the narrow-phase collision callback
void setNarrowPhaseCallback(NarrowPhaseCallback* callback) {
mNarrowPhaseCallback = callback;
}
/// Set the convex collision shape to test collision with
void setConvexShape(const ConvexShape* convexShape) {
mConvexShape = convexShape;
}
/// Set the concave collision shape
void setConcaveShape(const ConcaveShape* concaveShape) {
mConcaveShape = concaveShape;
}
/// Set the broadphase overlapping pair
void setOverlappingPair(OverlappingPair* overlappingPair) {
mOverlappingPair = overlappingPair;
}
/// Set the proxy shapes of the two collision shapes
void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) {
mConvexProxyShape = convexProxyShape;
mConcaveProxyShape = concaveProxyShape;
}
/// Test collision between a triangle and the convex mesh shape
virtual void testTriangle(const Vector3* trianglePoints);
};
// Class SmoothMeshContactInfo
/**
* This class is used to store data about a contact with a triangle for the smooth
* mesh algorithm.
*/
class SmoothMeshContactInfo {
public:
ContactPointInfo contactInfo;
bool isFirstShapeTriangle;
Vector3 triangleVertices[3];
/// Constructor
SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
const Vector3& trianglePoint2, const Vector3& trianglePoint3)
: contactInfo(contact) {
isFirstShapeTriangle = firstShapeTriangle;
triangleVertices[0] = trianglePoint1;
triangleVertices[1] = trianglePoint2;
triangleVertices[2] = trianglePoint3;
}
};
struct ContactsDepthCompare {
bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
{
return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
}
};
/// Method used to compare two smooth mesh contact info to sort them
//inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1,
// const SmoothMeshContactInfo& contact2) {
// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
//}
// Class SmoothCollisionNarrowPhaseCallback
/**
* This class is used as a narrow-phase callback to get narrow-phase contacts
* of the concave triangle mesh to temporary store them in order to be used in
* the smooth mesh collision algorithm if this one is enabled.
*/
class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
private:
std::vector<SmoothMeshContactInfo>& mContactPoints;
public:
// Constructor
SmoothCollisionNarrowPhaseCallback(std::vector<SmoothMeshContactInfo>& contactPoints)
: mContactPoints(contactPoints) {
}
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo);
};
// Class ConcaveVsConvexAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between a concave collision shape and a convex collision shape. The idea is
* to use the GJK collision detection algorithm to compute the collision between
* the convex shape and each of the triangles in the concave shape.
*/
class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
protected :
// -------------------- Attributes -------------------- //
// -------------------- Methods -------------------- //
/// Private copy-constructor
ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm);
/// Private assignment operator
ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm);
/// Process the concave triangle mesh collision using the smooth mesh collision algorithm
void processSmoothMeshCollision(OverlappingPair* overlappingPair,
std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback);
/// Add a triangle vertex into the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices,
const Vector3& vertex);
/// Return true if the vertex is in the set of already processed vertices
bool hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices,
const Vector3& vertex) const;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConcaveVsConvexAlgorithm();
/// Destructor
virtual ~ConcaveVsConvexAlgorithm();
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback);
};
// Add a triangle vertex into the set of processed triangles
inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) {
processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex));
}
}
#endif

59
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp Normal file
View File

@ -0,0 +1,59 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "collision/NarrowPhaseInfo.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two convex polyhedra
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
satAlgorithm.setProfiler(mProfiler);
#endif
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingSAT = true;
lastFrameCollisionInfo->wasUsingGJK = false;
return isColliding;
}

74
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h Normal file
View File

@ -0,0 +1,74 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
#define REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class Body;
class ContactPoint;
// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two convex polyhedra. Here we do not use the GJK algorithm but
* we run the SAT algorithm to get the contact points and normal.
* This is based on the "Robust Contact Creation for Physics Simulation"
* presentation by Dirk Gregorius.
*/
class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
ConvexPolyhedronVsConvexPolyhedronAlgorithm() = default;
/// Destructor
virtual ~ConvexPolyhedronVsConvexPolyhedronAlgorithm() override = default;
/// Deleted copy-constructor
ConvexPolyhedronVsConvexPolyhedronAlgorithm(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Deleted assignment operator
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

56
src/collision/narrowphase/DefaultCollisionDispatch.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -29,26 +29,6 @@
using namespace reactphysics3d;
// Constructor
DefaultCollisionDispatch::DefaultCollisionDispatch() {
}
// Destructor
DefaultCollisionDispatch::~DefaultCollisionDispatch() {
}
/// Initialize the collision dispatch configuration
void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
// Initialize the collision algorithms
mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
mGJKAlgorithm.init(collisionDetection, memoryAllocator);
mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator);
}
// Select and return the narrow-phase collision detection algorithm to
// use between two types of collision shapes.
NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
@ -56,20 +36,34 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
if (type1 > type2) {
return nullptr;
}
// Sphere vs Sphere algorithm
if (shape1Type == SPHERE && shape2Type == SPHERE) {
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
return &mSphereVsSphereAlgorithm;
}
// Concave vs Convex algorithm
else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
(!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
return &mConcaveVsConvexAlgorithm;
// Sphere vs Capsule algorithm
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CAPSULE) {
return &mSphereVsCapsuleAlgorithm;
}
// Convex vs Convex algorithm (GJK algorithm)
else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
return &mGJKAlgorithm;
// Capsule vs Capsule algorithm
if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) {
return &mCapsuleVsCapsuleAlgorithm;
}
else {
return NULL;
// Sphere vs Convex Polyhedron algorithm
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mSphereVsConvexPolyhedronAlgorithm;
}
// Capsule vs Convex Polyhedron algorithm
if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mCapsuleVsConvexPolyhedronAlgorithm;
}
// Convex Polyhedron vs Convex Polyhedron algorithm
if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON &&
shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mConvexPolyhedronVsConvexPolyhedronAlgorithm;
}
return nullptr;
}

61
src/collision/narrowphase/DefaultCollisionDispatch.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -28,9 +28,12 @@
// Libraries
#include "CollisionDispatch.h"
#include "ConcaveVsConvexAlgorithm.h"
#include "SphereVsSphereAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "SphereVsConvexPolyhedronAlgorithm.h"
#include "SphereVsCapsuleAlgorithm.h"
#include "CapsuleVsCapsuleAlgorithm.h"
#include "CapsuleVsConvexPolyhedronAlgorithm.h"
#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
namespace reactphysics3d {
@ -47,29 +50,59 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Sphere vs Sphere collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
/// Concave vs Convex collision algorithm
ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
/// Capsule vs Capsule collision algorithm
CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
/// GJK Algorithm
GJKAlgorithm mGJKAlgorithm;
/// Sphere vs Capsule collision algorithm
SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm;
/// Sphere vs Convex Polyhedron collision algorithm
SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
/// Capsule vs Convex Polyhedron collision algorithm
CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm;
/// Convex Polyhedron vs Convex Polyhedron collision algorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm;
public:
/// Constructor
DefaultCollisionDispatch();
DefaultCollisionDispatch() = default;
/// Destructor
virtual ~DefaultCollisionDispatch();
/// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator);
virtual ~DefaultCollisionDispatch() override = default;
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2);
virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler) override;
#endif
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void DefaultCollisionDispatch::setProfiler(Profiler* profiler) {
CollisionDispatch::setProfiler(profiler);
mSphereVsSphereAlgorithm.setProfiler(profiler);
mCapsuleVsCapsuleAlgorithm.setProfiler(profiler);
mSphereVsCapsuleAlgorithm.setProfiler(profiler);
mSphereVsConvexPolyhedronAlgorithm.setProfiler(profiler);
mCapsuleVsConvexPolyhedronAlgorithm.setProfiler(profiler);
mConvexPolyhedronVsConvexPolyhedronAlgorithm.setProfiler(profiler);
}
#endif
}
#endif

439
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -1,439 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "EPAAlgorithm.h"
#include "engine/Profiler.h"
#include "collision/narrowphase//GJK/GJKAlgorithm.h"
#include "TrianglesStore.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
EPAAlgorithm::EPAAlgorithm() {
}
// Destructor
EPAAlgorithm::~EPAAlgorithm() {
}
// Decide if the origin is in the tetrahedron.
/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
/// the vertex that is wrong if the origin is not in the tetrahedron
int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const {
// Check vertex 1
Vector3 normal1 = (p2-p1).cross(p3-p1);
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
return 4;
}
// Check vertex 2
Vector3 normal2 = (p4-p2).cross(p3-p2);
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
return 1;
}
// Check vertex 3
Vector3 normal3 = (p4-p3).cross(p1-p3);
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
return 2;
}
// Check vertex 4
Vector3 normal4 = (p2-p4).cross(p1-p4);
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
return 3;
}
// The origin is in the tetrahedron, we return 0
return 0;
}
// Compute the penetration depth with the EPA algorithm.
/// This method computes the penetration depth and contact points between two
/// enlarged objects (with margin) where the original objects (without margin)
/// intersect. An initial simplex that contains origin has been computed with
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
/// the correct penetration depth
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
CollisionShapeInfo shape2Info,
const Transform& transform2,
Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
Vector3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face
// candidate of the EPA algorithm
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local
// space of body 1 into local space of body 2
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm
unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance
decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope
unsigned int nbTriangles = 0;
// Clear the storing of triangles
triangleStore.clear();
// Select an action according to the number of points in the simplex
// computed with GJK algorithm in order to obtain an initial polytope for
// The EPA algorithm.
switch(nbVertices) {
case 1:
// Only one point in the simplex (which should be the origin).
// We have a touching contact with zero penetration depth.
// We drop that kind of contact. Therefore, we return false
return;
case 2: {
// The simplex returned by GJK is a line segment d containing the origin.
// We add two additional support points to construct a hexahedron (two tetrahedron
// glued together with triangle faces. The idea is to compute three different vectors
// v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively
// rotated of 120 degree around the d segment. The the three new points to
// construct the polytope are the three support points in those three directions
// v1, v2 and v3.
// Direction of the segment
Vector3 d = (points[1] - points[0]).getUnit();
// Choose the coordinate axis from the minimal absolute component of the vector d
int minAxis = d.getAbsoluteVector().getMinAxis();
// Compute sin(60)
const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5);
// Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3
Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5);
// Compute the vector v1, v2, v3
Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
Vector3 v2 = rotationQuat * v1;
Vector3 v3 = rotationQuat * v2;
// Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
suppPointsB[2] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
// tetrahedron that contains the origin in order that the initial polytope of the
// EPA algorithm is a tetrahedron, which is simpler to deal with.
// If the origin is in the tetrahedron of points 0, 2, 3, 4
if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) {
// We use the point 4 instead of point 1 for the initial tetrahedron
suppPointsA[1] = suppPointsA[4];
suppPointsB[1] = suppPointsB[4];
points[1] = points[4];
}
// If the origin is in the tetrahedron of points 1, 2, 3, 4
else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) {
// We use the point 4 instead of point 0 for the initial tetrahedron
suppPointsA[0] = suppPointsA[4];
suppPointsB[0] = suppPointsB[4];
points[0] = points[4];
}
else {
// The origin is not in the initial polytope
return;
}
// The polytope contains now 4 vertices
nbVertices = 4;
}
case 4: {
// The simplex computed by the GJK algorithm is a tetrahedron. Here we check
// if this tetrahedron contains the origin. If it is the case, we keep it and
// otherwise we remove the wrong vertex of the tetrahedron and go in the case
// where the GJK algorithm compute a simplex of three vertices.
// Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise)
int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]);
// If the origin is in the tetrahedron
if (badVertex == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron
TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2);
TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1);
TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
// If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return;
}
// Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
break;
}
// The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
// Remove the wrong vertex and continue to the next case with the
// three remaining vertices
if (badVertex < 4) {
suppPointsA[badVertex-1] = suppPointsA[3];
suppPointsB[badVertex-1] = suppPointsB[3];
points[badVertex-1] = points[3];
}
// We have removed the wrong vertex
nbVertices = 3;
}
case 3: {
// The GJK algorithm returned a triangle that contains the origin.
// We need two new vertices to create two tetrahedron. The two new
// vertices are the support points in the "n" and "-n" direction
// where "n" is the normal of the triangle. Then, we use only the
// tetrahedron that contains the origin.
// Compute the normal of the triangle
Vector3 v1 = points[1] - points[0];
Vector3 v2 = points[2] - points[0];
Vector3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0 = NULL;
TriangleEPA* face1 = NULL;
TriangleEPA* face2 = NULL;
TriangleEPA* face3 = NULL;
// If the origin is in the first tetrahedron
if (isOriginInTetrahedron(points[0], points[1],
points[2], points[3]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 3, 1);
face2 = triangleStore.newTriangle(points, 0, 2, 3);
face3 = triangleStore.newTriangle(points, 1, 3, 2);
}
else if (isOriginInTetrahedron(points[0], points[1],
points[2], points[4]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 4, 1);
face2 = triangleStore.newTriangle(points, 0, 2, 4);
face3 = triangleStore.newTriangle(points, 1, 4, 2);
}
else {
return;
}
// If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return;
}
// Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
nbVertices = 4;
}
break;
}
// At this point, we have a polytope that contains the origin. Therefore, we
// can run the EPA algorithm.
if (nbTriangles == 0) {
return;
}
TriangleEPA* triangle = 0;
decimal upperBoundSquarePenDepth = DECIMAL_LARGEST;
do {
triangle = triangleHeap[0];
// Get the next candidate face (the face closest to the origin)
std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison);
nbTriangles--;
// If the candidate face in the heap is not obsolete
if (!triangle->getIsObsolete()) {
// If we have reached the maximum number of support points
if (nbVertices == MAX_SUPPORT_POINTS) {
assert(false);
break;
}
// Compute the support point of the Minkowski
// difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(
triangle->getClosestPoint(), shape1CachedCollisionData);
suppPointsB[nbVertices] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices;
nbVertices++;
// Update the upper bound of the penetration depth
decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
assert(wDotv > 0.0);
decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare;
}
// Compute the error
decimal error = wDotv - triangle->getDistSquare();
if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
points[indexNewVertex] == points[(*triangle)[0]] ||
points[indexNewVertex] == points[(*triangle)[1]] ||
points[indexNewVertex] == points[(*triangle)[2]]) {
break;
}
// Now, we compute the silhouette cast by the new vertex. The current triangle
// face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face.
int i = triangleStore.getNbTriangles();
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
break;
}
// Add all the new triangle faces computed with the silhouette algorithm
// to the candidates list of faces of the current polytope
while(i != triangleStore.getNbTriangles()) {
TriangleEPA* newTriangle = &triangleStore[i];
addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
i++;
}
}
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info
v = transform1.getOrientation() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
if (normal.lengthSquare() < MACHINE_EPSILON) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
}

161
src/collision/narrowphase/EPA/EPAAlgorithm.h
View File

@ -1,161 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_EPA_ALGORITHM_H
#define REACTPHYSICS3D_EPA_ALGORITHM_H
// Libraries
#include "collision/narrowphase/GJK/Simplex.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/CollisionShapeInfo.h"
#include "constraint/ContactPoint.h"
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "mathematics/mathematics.h"
#include "TriangleEPA.h"
#include "memory/MemoryAllocator.h"
#include <algorithm>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// ---------- Constants ---------- //
/// Maximum number of support points of the polytope
const unsigned int MAX_SUPPORT_POINTS = 100;
/// Maximum number of facets of the polytope
const unsigned int MAX_FACETS = 200;
// Class TriangleComparison
/**
* This class allows the comparison of two triangles in the heap
* The comparison between two triangles is made using their square distance to the closest
* point to the origin. The goal is that in the heap, the first triangle is the one with the
* smallest square distance.
*/
class TriangleComparison {
public:
/// Comparison operator
bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) {
return (face1->getDistSquare() > face2->getDistSquare());
}
};
// Class EPAAlgorithm
/**
* This class is the implementation of the Expanding Polytope Algorithm (EPA).
* The EPA algorithm computes the penetration depth and contact points between
* two enlarged objects (with margin) where the original objects (without margin)
* intersect. The penetration depth of a pair of intersecting objects A and B is
* the length of a point on the boundary of the Minkowski sum (A-B) closest to the
* origin. The goal of the EPA algorithm is to start with an initial simplex polytope
* that contains the origin and expend it in order to find the point on the boundary
* of (A-B) that is closest to the origin. An initial simplex that contains origin
* has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex
* polytope to find the correct penetration depth. The implementation of the EPA
* algorithm is based on the book "Collision Detection in 3D Environments".
*/
class EPAAlgorithm {
private:
// -------------------- Attributes -------------------- //
/// Reference to the memory allocator
MemoryAllocator* mMemoryAllocator;
/// Triangle comparison operator
TriangleComparison mTriangleComparison;
// -------------------- Methods -------------------- //
/// Private copy-constructor
EPAAlgorithm(const EPAAlgorithm& algorithm);
/// Private assignment operator
EPAAlgorithm& operator=(const EPAAlgorithm& algorithm);
/// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles,
decimal upperBoundSquarePenDepth);
/// Decide if the origin is in the tetrahedron.
int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const;
public:
// -------------------- Methods -------------------- //
/// Constructor
EPAAlgorithm();
/// Destructor
~EPAAlgorithm();
/// Initalize the algorithm
void init(MemoryAllocator* memoryAllocator);
/// Compute the penetration depth with EPA algorithm.
void computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
CollisionShapeInfo shape2Info,
const Transform& transform2,
Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback);
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm
inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
uint& nbTriangles, decimal upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle
// points is internal to the triangle and if the distance
// of the closest point from the origin is at most the
// penetration depth upper bound
if (triangle->isClosestPointInternalToTriangle() &&
triangle->getDistSquare() <= upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates
heap[nbTriangles] = triangle;
nbTriangles++;
std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison);
}
}
// Initalize the algorithm
inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) {
mMemoryAllocator = memoryAllocator;
}
}
#endif

136
src/collision/narrowphase/EPA/EdgeEPA.cpp
View File

@ -1,136 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "EdgeEPA.h"
#include "TriangleEPA.h"
#include "TrianglesStore.h"
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
EdgeEPA::EdgeEPA() {
}
// Constructor
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index)
: mOwnerTriangle(ownerTriangle), mIndex(index) {
assert(index >= 0 && index < 3);
}
// Copy-constructor
EdgeEPA::EdgeEPA(const EdgeEPA& edge) {
mOwnerTriangle = edge.mOwnerTriangle;
mIndex = edge.mIndex;
}
// Destructor
EdgeEPA::~EdgeEPA() {
}
// Return the index of the source vertex of the edge (vertex starting the edge)
uint EdgeEPA::getSourceVertexIndex() const {
return (*mOwnerTriangle)[mIndex];
}
// Return the index of the target vertex of the edge (vertex ending the edge)
uint EdgeEPA::getTargetVertexIndex() const {
return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)];
}
// Execute the recursive silhouette algorithm from this edge
bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
TrianglesStore& triangleStore) {
// If the edge has not already been visited
if (!mOwnerTriangle->getIsObsolete()) {
// If the triangle of this edge is not visible from the given point
if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
// If the triangle has been created
if (triangle != NULL) {
halfLink(EdgeEPA(triangle, 1), *this);
return true;
}
return false;
}
else {
// The current triangle is visible and therefore obsolete
mOwnerTriangle->setIsObsolete(true);
int backup = triangleStore.getNbTriangles();
if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
this->mIndex)).computeSilhouette(vertices,
indexNewVertex,
triangleStore)) {
mOwnerTriangle->setIsObsolete(false);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
// If the triangle has been created
if (triangle != NULL) {
halfLink(EdgeEPA(triangle, 1), *this);
return true;
}
return false;
}
else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(
this->mIndex)).computeSilhouette(vertices,
indexNewVertex,
triangleStore)) {
mOwnerTriangle->setIsObsolete(false);
triangleStore.setNbTriangles(backup);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
if (triangle != NULL) {
halfLink(EdgeEPA(triangle, 1), *this);
return true;
}
return false;
}
}
}
return true;
}

155
src/collision/narrowphase/EPA/TriangleEPA.cpp
View File

@ -1,155 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "TriangleEPA.h"
#include "EdgeEPA.h"
#include "TrianglesStore.h"
// We use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
TriangleEPA::TriangleEPA() {
}
// Constructor
TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3)
: mIsObsolete(false) {
mIndicesVertices[0] = indexVertex1;
mIndicesVertices[1] = indexVertex2;
mIndicesVertices[2] = indexVertex3;
}
// Destructor
TriangleEPA::~TriangleEPA() {
}
// Compute the point v closest to the origin of this triangle
bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
const Vector3& p0 = vertices[mIndicesVertices[0]];
Vector3 v1 = vertices[mIndicesVertices[1]] - p0;
Vector3 v2 = vertices[mIndicesVertices[2]] - p0;
decimal v1Dotv1 = v1.dot(v1);
decimal v1Dotv2 = v1.dot(v2);
decimal v2Dotv2 = v2.dot(v2);
decimal p0Dotv1 = p0.dot(v1);
decimal p0Dotv2 = p0.dot(v2);
// Compute determinant
mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
// Compute lambda values
mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2;
mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1;
// If the determinant is positive
if (mDet > 0.0) {
// Compute the closest point v
mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2);
// Compute the square distance of closest point to the origin
mDistSquare = mClosestPoint.dot(mClosestPoint);
return true;
}
return false;
}
/// Link an edge with another one. It means that the current edge of a triangle will
/// be associated with the edge of another triangle in order that both triangles
/// are neighbour along both edges).
bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
if (isPossible) {
edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0;
}
return isPossible;
}
/// Make an half link of an edge with another one from another triangle. An half-link
/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an
/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
/// be made later.
void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
// Link
edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
}
// Execute the recursive silhouette algorithm from this triangle face.
/// The parameter "vertices" is an array that contains the vertices of the current polytope and the
/// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the
/// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore,
/// the triangle faces that are visible from the new vertex must be removed from the polytope and we
/// need to add triangle faces where each face contains the new vertex and an edge of the silhouette.
/// The silhouette is the connected set of edges that are part of the border between faces that
/// are seen and faces that are not seen from the new vertex. This method starts from the nearest
/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
/// order that we always have a convex polytope. The faces visible from the new vertex are set
/// obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
TrianglesStore& triangleStore) {
uint first = triangleStore.getNbTriangles();
// Mark the current triangle as obsolete because it
setIsObsolete(true);
// Execute recursively the silhouette algorithm for the adjacent edges of neighboring
// triangles of the current triangle
bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
if (result) {
int i,j;
// For each triangle face that contains the new vertex and an edge of the silhouette
for (i=first, j=triangleStore.getNbTriangles()-1;
i != triangleStore.getNbTriangles(); j = i++) {
TriangleEPA* triangle = &triangleStore[i];
halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));
if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) {
return false;
}
}
}
return result;
}

199
src/collision/narrowphase/EPA/TriangleEPA.h
View File

@ -1,199 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_TRIANGLE_EPA_H
#define REACTPHYSICS3D_TRIANGLE_EPA_H
// Libraries
#include "mathematics/mathematics.h"
#include "configuration.h"
#include "EdgeEPA.h"
#include <cassert>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Prototypes
bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
// Class TriangleEPA
/**
* This class represents a triangle face of the current polytope in the EPA algorithm.
*/
class TriangleEPA {
private:
// -------------------- Attributes -------------------- //
/// Indices of the vertices y_i of the triangle
uint mIndicesVertices[3];
/// Three adjacent edges of the triangle (edges of other triangles)
EdgeEPA mAdjacentEdges[3];
/// True if the triangle face is visible from the new support point
bool mIsObsolete;
/// Determinant
decimal mDet;
/// Point v closest to the origin on the affine hull of the triangle
Vector3 mClosestPoint;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal mLambda1;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal mLambda2;
/// Square distance of the point closest point v to the origin
decimal mDistSquare;
// -------------------- Methods -------------------- //
/// Private copy-constructor
TriangleEPA(const TriangleEPA& triangle);
/// Private assignment operator
TriangleEPA& operator=(const TriangleEPA& triangle);
public:
// -------------------- Methods -------------------- //
/// Constructor
TriangleEPA();
/// Constructor
TriangleEPA(uint v1, uint v2, uint v3);
/// Destructor
~TriangleEPA();
/// Return an adjacent edge of the triangle
EdgeEPA& getAdjacentEdge(int index);
/// Set an adjacent edge of the triangle
void setAdjacentEdge(int index, EdgeEPA& edge);
/// Return the square distance of the closest point to origin
decimal getDistSquare() const;
/// Set the isObsolete value
void setIsObsolete(bool isObsolete);
/// Return true if the triangle face is obsolete
bool getIsObsolete() const;
/// Return the point closest to the origin
const Vector3& getClosestPoint() const;
// Return true if the closest point on affine hull is inside the triangle
bool isClosestPointInternalToTriangle() const;
/// Return true if the triangle is visible from a given vertex
bool isVisibleFromVertex(const Vector3* vertices, uint index) const;
/// Compute the point v closest to the origin of this triangle
bool computeClosestPoint(const Vector3* vertices);
/// Compute the point of an object closest to the origin
Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const;
/// Execute the recursive silhouette algorithm from this triangle face.
bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore);
/// Access operator
uint operator[](int i) const;
/// Associate two edges
friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
/// Make a half-link between two edges
friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
};
// Return an edge of the triangle
inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) {
assert(index >= 0 && index < 3);
return mAdjacentEdges[index];
}
// Set an adjacent edge of the triangle
inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) {
assert(index >=0 && index < 3);
mAdjacentEdges[index] = edge;
}
// Return the square distance of the closest point to origin
inline decimal TriangleEPA::getDistSquare() const {
return mDistSquare;
}
// Set the isObsolete value
inline void TriangleEPA::setIsObsolete(bool isObsolete) {
mIsObsolete = isObsolete;
}
// Return true if the triangle face is obsolete
inline bool TriangleEPA::getIsObsolete() const {
return mIsObsolete;
}
// Return the point closest to the origin
inline const Vector3& TriangleEPA::getClosestPoint() const {
return mClosestPoint;
}
// Return true if the closest point on affine hull is inside the triangle
inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet);
}
// Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const {
Vector3 closestToVert = vertices[index] - mClosestPoint;
return (mClosestPoint.dot(closestToVert) > 0.0);
}
// Compute the point of an object closest to the origin
inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{
const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]];
return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
}
// Access operator
inline uint TriangleEPA::operator[](int i) const {
assert(i >= 0 && i <3);
return mIndicesVertices[i];
}
}
#endif

141
src/collision/narrowphase/EPA/TrianglesStore.h
View File

@ -1,141 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_TRIANGLES_STORE_H
#define REACTPHYSICS3D_TRIANGLES_STORE_H
#include "TriangleEPA.h"
// Libraries
#include <cassert>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles
// Class TriangleStore
/**
* This class stores several triangles of the polytope in the EPA algorithm.
*/
class TrianglesStore {
private:
// -------------------- Attributes -------------------- //
/// Triangles
TriangleEPA mTriangles[MAX_TRIANGLES];
/// Number of triangles
int mNbTriangles;
// -------------------- Methods -------------------- //
/// Private copy-constructor
TrianglesStore(const TrianglesStore& triangleStore);
/// Private assignment operator
TrianglesStore& operator=(const TrianglesStore& triangleStore);
public:
// -------------------- Methods -------------------- //
/// Constructor
TrianglesStore();
/// Destructor
~TrianglesStore();
/// Clear all the storage
void clear();
/// Return the number of triangles
int getNbTriangles() const;
/// Set the number of triangles
void setNbTriangles(int backup);
/// Return the last triangle
TriangleEPA& last();
/// Create a new triangle
TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2);
/// Access operator
TriangleEPA& operator[](int i);
};
// Clear all the storage
inline void TrianglesStore::clear() {
mNbTriangles = 0;
}
// Return the number of triangles
inline int TrianglesStore::getNbTriangles() const {
return mNbTriangles;
}
inline void TrianglesStore::setNbTriangles(int backup) {
mNbTriangles = backup;
}
// Return the last triangle
inline TriangleEPA& TrianglesStore::last() {
assert(mNbTriangles > 0);
return mTriangles[mNbTriangles - 1];
}
// Create a new triangle
inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices,
uint v0,uint v1, uint v2) {
TriangleEPA* newTriangle = NULL;
// If we have not reached the maximum number of triangles
if (mNbTriangles != MAX_TRIANGLES) {
newTriangle = &mTriangles[mNbTriangles++];
new (newTriangle) TriangleEPA(v0, v1, v2);
if (!newTriangle->computeClosestPoint(vertices)) {
mNbTriangles--;
newTriangle = NULL;
}
}
// Return the new triangle
return newTriangle;
}
// Access operator
inline TriangleEPA& TrianglesStore::operator[](int i) {
return mTriangles[i];
}
}
#endif

481
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -25,28 +25,18 @@
// Libraries
#include "GJKAlgorithm.h"
#include "Simplex.h"
#include "constraint/ContactPoint.h"
#include "engine/OverlappingPair.h"
#include "collision/shapes/TriangleShape.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
#include <cmath>
#include <cfloat>
#include "utils/Profiler.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/narrowphase/GJK/VoronoiSimplex.h"
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
}
// Destructor
GJKAlgorithm::~GJKAlgorithm() {
}
// Compute a contact info if the two collision shapes collide.
/// This method implements the Hybrid Technique for computing the penetration depth by
/// running the GJK algorithm on original objects (without margin). If the shapes intersect
@ -56,11 +46,9 @@ GJKAlgorithm::~GJKAlgorithm() {
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
PROFILE("GJKAlgorithm::testCollision()");
RP3D_PROFILE("GJKAlgorithm::testCollision()", mProfiler);
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
@ -69,39 +57,47 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Vector3 pB; // Closest point of object B
decimal vDotw;
decimal prevDistSquare;
bool contactFound = false;
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
assert(narrowPhaseInfo->collisionShape1->isConvex());
assert(narrowPhaseInfo->collisionShape2->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
// Get the local-space to world-space transforms
const Transform transform1 = shape1Info.shapeToWorldTransform;
const Transform transform2 = shape2Info.shapeToWorldTransform;
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2;
Transform transform1Inverse = transform1.getInverse();
Transform body2Tobody1 = transform1Inverse * transform2;
// Matrix that transform a direction from local
// Quaternion that transform a direction from local
// space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix();
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation();
// Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin;
assert(margin > 0.0);
assert(margin > decimal(0.0));
// Create a simplex set
Simplex simplex;
VoronoiSimplex simplex;
// Get the last collision frame info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
// Get the previous point V (last cached separating axis)
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
Vector3 v;
if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) {
v = lastFrameCollisionInfo->gjkSeparatingAxis;
assert(v.lengthSquare() > decimal(0.000001));
}
else {
v.setAllValues(0, 1, 0);
}
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
@ -109,9 +105,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
do {
// Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
suppB = body2Tobody1 *
shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
suppA = shape1->getLocalSupportPointWithoutMargin(-v);
suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -119,43 +114,21 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
vDotw = v.dot(w);
// If the enlarge objects (with margins) do not intersect
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
mCurrentOverlappingPair->setCachedSeparatingAxis(v);
lastFrameCollisionInfo->gjkSeparatingAxis = v;
// No intersection, we return
return;
return GJKResult::SEPARATED;
}
// If the objects intersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
// Contact point has been found
contactFound = true;
break;
}
// Add the new support point to the simplex
@ -164,62 +137,18 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
// Contact point has been found
contactFound = true;
break;
}
// Compute the point of the simplex closest to the origin
// If the computation of the closest point fail
// If the computation of the closest point fails
if (!simplex.computeClosestPoint(v)) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
// Contact point has been found
contactFound = true;
break;
}
// Store and update the squared distance of the closest point
@ -228,301 +157,57 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
distSquare = v.lengthSquare();
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint());
// The objects (without margins) intersect. Therefore, we run the GJK algorithm
// again but on the enlarged objects to compute a simplex polytope that contains
// the origin. Then, we give that simplex polytope to the EPA algorithm to compute
// the correct penetration depth and contact points between the enlarged objects.
return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
transform2, narrowPhaseCallback, v);
}
/// This method runs the GJK algorithm on the two enlarged objects (with margin)
/// to compute a simplex polytope that contains the origin. The two objects are
/// assumed to intersect in the original objects (without margin). Therefore such
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
/// compute the correct penetration depth and contact points of the enlarged objects.
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
Simplex simplex;
Vector3 suppA;
Vector3 suppB;
Vector3 w;
decimal vDotw;
decimal distSquare = DECIMAL_LARGEST;
decimal prevDistSquare;
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// Transform a point from local space of body 2 to local space
// of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2ToBody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix();
do {
// Compute the support points for the enlarged object A and B
suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
vDotw = v.dot(w);
// If the enlarge objects do not intersect
if (vDotw > 0.0) {
// No intersection, we return
return;
// Contact point has been found
contactFound = true;
break;
}
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
if (simplex.isAffinelyDependent()) {
return;
if (contactFound && distSquare > MACHINE_EPSILON) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = std::sqrt(distSquare);
assert(dist > decimal(0.0));
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// If the penetration depth is negative (due too numerical errors), there is no contact
if (penetrationDepth <= decimal(0.0)) {
return GJKResult::SEPARATED;
}
if (!simplex.computeClosestPoint(v)) {
return;
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
return GJKResult::SEPARATED;
}
// Store and update the square distance
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
if (reportContacts) {
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2,
penetrationDepth, normal);
// Add a new contact point
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint());
// Give the simplex computed with GJK algorithm to the EPA algorithm
// which will compute the correct penetration depth and contact points
// between the two enlarged objects
return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
transform1, shape2Info, transform2,
v, narrowPhaseCallback);
}
// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
Vector3 suppA; // Support point of object A
Vector3 w; // Support point of Minkowski difference A-B
decimal prevDistSquare;
assert(proxyShape->getCollisionShape()->isConvex());
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
// Support point of object B (object B is a single point)
const Vector3 suppB(localPoint);
// Create a simplex set
Simplex simplex;
// Initial supporting direction
Vector3 v(1, 1, 1);
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
do {
// Compute the support points for original objects (without margins) A and B
suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) {
return false;
}
// Compute the point of the simplex closest to the origin
// If the computation of the closest point fail
if (!simplex.computeClosestPoint(v)) {
return false;
}
// Store and update the squared distance of the closest point
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return false;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint());
// The point is inside the collision shape
return true;
}
// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) {
assert(proxyShape->getCollisionShape()->isConvex());
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape
const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
const decimal epsilon = decimal(0.0001);
// Convert the ray origin and direction into the local-space of the collision shape
Vector3 rayDirection = ray.point2 - ray.point1;
// If the points of the segment are two close, return no hit
if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
Vector3 w;
// Create a simplex set
Simplex simplex;
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
decimal distSquare = v.lengthSquare();
int nbIterations = 0;
// GJK Algorithm loop
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
// Compute the support points
suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
w = suppA - suppB;
vDotW = v.dot(w);
if (vDotW > decimal(0)) {
vDotR = v.dot(rayDirection);
if (vDotR >= -machineEpsilonSquare) {
return false;
}
else {
// We have found a better lower bound for the hit point along the ray
lambda = lambda - vDotW / vDotR;
suppA = ray.point1 + lambda * rayDirection;
w = suppA - suppB;
n = v;
}
}
// Add the new support point to the simplex
if (!simplex.isPointInSimplex(w)) {
simplex.addPoint(w, suppA, suppB);
}
// Compute the closest point
if (simplex.computeClosestPoint(v)) {
distSquare = v.lengthSquare();
}
else {
distSquare = decimal(0.0);
}
// If the current lower bound distance is larger than the maximum raycasting distance
if (lambda > ray.maxFraction) return false;
nbIterations++;
return GJKResult::COLLIDE_IN_MARGIN;
}
// If the origin was inside the shape, we return no hit
if (lambda < MACHINE_EPSILON) return false;
// Compute the closet points of both objects (without the margins)
Vector3 pointA;
Vector3 pointB;
simplex.computeClosestPointsOfAandB(pointA, pointB);
// A raycast hit has been found, we fill in the raycast info
raycastInfo.hitFraction = lambda;
raycastInfo.worldPoint = pointB;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = n;
}
else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0));
}
return true;
return GJKResult::INTERPENETRATE;
}

98
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,94 +27,94 @@
#define REACTPHYSICS3D_GJK_ALGORITHM_H
// Libraries
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "collision/shapes/ConvexShape.h"
#include "collision/narrowphase/EPA/EPAAlgorithm.h"
#include "decimal.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class ContactManifoldInfo;
struct NarrowPhaseInfo;
class ConvexShape;
class Profiler;
class VoronoiSimplex;
// Constants
const decimal REL_ERROR = decimal(1.0e-3);
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const int MAX_ITERATIONS_GJK_RAYCAST = 32;
constexpr decimal REL_ERROR = decimal(1.0e-3);
constexpr decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
constexpr int MAX_ITERATIONS_GJK_RAYCAST = 32;
// Class GJKAlgorithm
/**
* This class implements a narrow-phase collision detection algorithm. This
* algorithm uses the ISA-GJK algorithm and the EPA algorithm. This
* algorithm uses the ISA-GJK algorithm. This
* implementation is based on the implementation discussed in the book
* "Collision Detection in Interactive 3D Environments" by Gino van den Bergen.
* This method implements the Hybrid Technique for calculating the
* penetration depth. The two objects are enlarged with a small margin. If
* the object intersects in their margins, the penetration depth is quickly
* computed using the GJK algorithm on the original objects (without margin).
* If the original objects (without margin) intersect, we run again the GJK
* algorithm on the enlarged objects (with margin) to compute simplex
* polytope that contains the origin and give it to the EPA (Expanding
* Polytope Algorithm) to compute the correct penetration depth between the
* enlarged objects.
* If the original objects (without margin) intersect, we exit GJK and run
* the SAT algorithm to get contacts and collision data.
*/
class GJKAlgorithm : public NarrowPhaseAlgorithm {
class GJKAlgorithm {
private :
// -------------------- Attributes -------------------- //
/// EPA Algorithm
EPAAlgorithm mAlgoEPA;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Private copy-constructor
GJKAlgorithm(const GJKAlgorithm& algorithm);
/// Private assignment operator
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
/// Compute the penetration depth for enlarged objects.
void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v);
public :
enum class GJKResult {
SEPARATED, // The two shapes are separated outside the margin
COLLIDE_IN_MARGIN, // The two shapes overlap only in the margin (shallow penetration)
INTERPENETRATE // The two shapes overlap event without the margin (deep penetration)
};
// -------------------- Methods -------------------- //
/// Constructor
GJKAlgorithm();
GJKAlgorithm() = default;
/// Destructor
~GJKAlgorithm();
~GJKAlgorithm() = default;
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator);
/// Deleted copy-constructor
GJKAlgorithm(const GJKAlgorithm& algorithm) = delete;
/// Deleted assignment operator
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volumes collide.
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback);
GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
};
// Initalize the algorithm
inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
mAlgoEPA.init(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void GJKAlgorithm::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

394
src/collision/narrowphase/GJK/Simplex.cpp
View File

@ -1,394 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "Simplex.h"
#include <cfloat>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Simplex::Simplex() : mBitsCurrentSimplex(0x0), mAllBits(0x0) {
}
// Destructor
Simplex::~Simplex() {
}
// Add a new support point of (A-B) into the simplex
/// suppPointA : support point of object A in a direction -v
/// suppPointB : support point of object B in a direction v
/// point : support point of object (A-B) => point = suppPointA - suppPointB
void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) {
assert(!isFull());
mLastFound = 0;
mLastFoundBit = 0x1;
// Look for the bit corresponding to one of the four point that is not in
// the current simplex
while (overlap(mBitsCurrentSimplex, mLastFoundBit)) {
mLastFound++;
mLastFoundBit <<= 1;
}
assert(mLastFound < 4);
// Add the point into the simplex
mPoints[mLastFound] = point;
mPointsLengthSquare[mLastFound] = point.dot(point);
mAllBits = mBitsCurrentSimplex | mLastFoundBit;
// Update the cached values
updateCache();
// Compute the cached determinant values
computeDeterminants();
// Add the support points of objects A and B
mSuppPointsA[mLastFound] = suppPointA;
mSuppPointsB[mLastFound] = suppPointB;
}
// Return true if the point is in the simplex
bool Simplex::isPointInSimplex(const Vector3& point) const {
int i;
Bits bit;
// For each four possible points in the simplex
for (i=0, bit = 0x1; i<4; i++, bit <<= 1) {
// Check if the current point is in the simplex
if (overlap(mAllBits, bit) && point == mPoints[i]) return true;
}
return false;
}
// Update the cached values used during the GJK algorithm
void Simplex::updateCache() {
int i;
Bits bit;
// For each of the four possible points of the simplex
for (i=0, bit = 0x1; i<4; i++, bit <<= 1) {
// If the current points is in the simplex
if (overlap(mBitsCurrentSimplex, bit)) {
// Compute the distance between two points in the possible simplex set
mDiffLength[i][mLastFound] = mPoints[i] - mPoints[mLastFound];
mDiffLength[mLastFound][i] = -mDiffLength[i][mLastFound];
// Compute the squared length of the vector
// distances from points in the possible simplex set
mNormSquare[i][mLastFound] = mNormSquare[mLastFound][i] =
mDiffLength[i][mLastFound].dot(mDiffLength[i][mLastFound]);
}
}
}
// Return the points of the simplex
unsigned int Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB,
Vector3* points) const {
unsigned int nbVertices = 0;
int i;
Bits bit;
// For each four point in the possible simplex
for (i=0, bit=0x1; i<4; i++, bit <<=1) {
// If the current point is in the simplex
if (overlap(mBitsCurrentSimplex, bit)) {
// Store the points
suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices];
suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices];
points[nbVertices] = this->mPoints[nbVertices];
nbVertices++;
}
}
// Return the number of points in the simplex
return nbVertices;
}
// Compute the cached determinant values
void Simplex::computeDeterminants() {
mDet[mLastFoundBit][mLastFound] = 1.0;
// If the current simplex is not empty
if (!isEmpty()) {
int i;
Bits bitI;
// For each possible four points in the simplex set
for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) {
// If the current point is in the simplex
if (overlap(mBitsCurrentSimplex, bitI)) {
Bits bit2 = bitI | mLastFoundBit;
mDet[bit2][i] = mDiffLength[mLastFound][i].dot(mPoints[mLastFound]);
mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]);
int j;
Bits bitJ;
for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) {
if (overlap(mBitsCurrentSimplex, bitJ)) {
int k;
Bits bit3 = bitJ | bit2;
k = mNormSquare[i][j] < mNormSquare[mLastFound][j] ? i : mLastFound;
mDet[bit3][j] = mDet[bit2][i] * mDiffLength[k][j].dot(mPoints[i]) +
mDet[bit2][mLastFound] *
mDiffLength[k][j].dot(mPoints[mLastFound]);
k = mNormSquare[j][i] < mNormSquare[mLastFound][i] ? j : mLastFound;
mDet[bit3][i] = mDet[bitJ | mLastFoundBit][j] *
mDiffLength[k][i].dot(mPoints[j]) +
mDet[bitJ | mLastFoundBit][mLastFound] *
mDiffLength[k][i].dot(mPoints[mLastFound]);
k = mNormSquare[i][mLastFound] < mNormSquare[j][mLastFound] ? i : j;
mDet[bit3][mLastFound] = mDet[bitJ | bitI][j] *
mDiffLength[k][mLastFound].dot(mPoints[j]) +
mDet[bitJ | bitI][i] *
mDiffLength[k][mLastFound].dot(mPoints[i]);
}
}
}
}
if (mAllBits == 0xf) {
int k;
k = mNormSquare[1][0] < mNormSquare[2][0] ?
(mNormSquare[1][0] < mNormSquare[3][0] ? 1 : 3) :
(mNormSquare[2][0] < mNormSquare[3][0] ? 2 : 3);
mDet[0xf][0] = mDet[0xe][1] * mDiffLength[k][0].dot(mPoints[1]) +
mDet[0xe][2] * mDiffLength[k][0].dot(mPoints[2]) +
mDet[0xe][3] * mDiffLength[k][0].dot(mPoints[3]);
k = mNormSquare[0][1] < mNormSquare[2][1] ?
(mNormSquare[0][1] < mNormSquare[3][1] ? 0 : 3) :
(mNormSquare[2][1] < mNormSquare[3][1] ? 2 : 3);
mDet[0xf][1] = mDet[0xd][0] * mDiffLength[k][1].dot(mPoints[0]) +
mDet[0xd][2] * mDiffLength[k][1].dot(mPoints[2]) +
mDet[0xd][3] * mDiffLength[k][1].dot(mPoints[3]);
k = mNormSquare[0][2] < mNormSquare[1][2] ?
(mNormSquare[0][2] < mNormSquare[3][2] ? 0 : 3) :
(mNormSquare[1][2] < mNormSquare[3][2] ? 1 : 3);
mDet[0xf][2] = mDet[0xb][0] * mDiffLength[k][2].dot(mPoints[0]) +
mDet[0xb][1] * mDiffLength[k][2].dot(mPoints[1]) +
mDet[0xb][3] * mDiffLength[k][2].dot(mPoints[3]);
k = mNormSquare[0][3] < mNormSquare[1][3] ?
(mNormSquare[0][3] < mNormSquare[2][3] ? 0 : 2) :
(mNormSquare[1][3] < mNormSquare[2][3] ? 1 : 2);
mDet[0xf][3] = mDet[0x7][0] * mDiffLength[k][3].dot(mPoints[0]) +
mDet[0x7][1] * mDiffLength[k][3].dot(mPoints[1]) +
mDet[0x7][2] * mDiffLength[k][3].dot(mPoints[2]);
}
}
}
// Return true if the subset is a proper subset.
/// A proper subset X is a subset where for all point "y_i" in X, we have
/// detX_i value bigger than zero
bool Simplex::isProperSubset(Bits subset) const {
int i;
Bits bit;
// For each four point of the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<=1) {
if (overlap(subset, bit) && mDet[subset][i] <= 0.0) {
return false;
}
}
return true;
}
// Return true if the set is affinely dependent.
/// A set if affinely dependent if a point of the set
/// is an affine combination of other points in the set
bool Simplex::isAffinelyDependent() const {
decimal sum = 0.0;
int i;
Bits bit;
// For each four point of the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
if (overlap(mAllBits, bit)) {
sum += mDet[mAllBits][i];
}
}
return (sum <= 0.0);
}
// Return true if the subset is a valid one for the closest point computation.
/// A subset X is valid if :
/// 1. delta(X)_i > 0 for each "i" in I_x and
/// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_
bool Simplex::isValidSubset(Bits subset) const {
int i;
Bits bit;
// For each four point in the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
if (overlap(mAllBits, bit)) {
// If the current point is in the subset
if (overlap(subset, bit)) {
// If one delta(X)_i is smaller or equal to zero
if (mDet[subset][i] <= 0.0) {
// The subset is not valid
return false;
}
}
// If the point is not in the subset and the value delta(X U {y_j})_j
// is bigger than zero
else if (mDet[subset | bit][i] > 0.0) {
// The subset is not valid
return false;
}
}
}
return true;
}
// Compute the closest points "pA" and "pB" of object A and B.
/// The points are computed as follows :
/// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A
/// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B
/// with lambda_i = deltaX_i / deltaX
void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
decimal deltaX = 0.0;
pA.setAllValues(0.0, 0.0, 0.0);
pB.setAllValues(0.0, 0.0, 0.0);
int i;
Bits bit;
// For each four points in the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
// If the current point is part of the simplex
if (overlap(mBitsCurrentSimplex, bit)) {
deltaX += mDet[mBitsCurrentSimplex][i];
pA += mDet[mBitsCurrentSimplex][i] * mSuppPointsA[i];
pB += mDet[mBitsCurrentSimplex][i] * mSuppPointsB[i];
}
}
assert(deltaX > 0.0);
decimal factor = decimal(1.0) / deltaX;
pA *= factor;
pB *= factor;
}
// Compute the closest point "v" to the origin of the current simplex.
/// This method executes the Jonhnson's algorithm for computing the point
/// "v" of simplex that is closest to the origin. The method returns true
/// if a closest point has been found.
bool Simplex::computeClosestPoint(Vector3& v) {
Bits subset;
// For each possible simplex set
for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) {
// If the simplex is a subset of the current simplex and is valid for the Johnson's
// algorithm test
if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) {
mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex
v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex
return true;
}
}
// If the simplex that contains only the last added point is valid for the Johnson's algorithm test
if (isValidSubset(mLastFoundBit)) {
mBitsCurrentSimplex = mLastFoundBit; // Set the current simplex to the set that contains only the last added point
mMaxLengthSquare = mPointsLengthSquare[mLastFound]; // Update the maximum square length
v = mPoints[mLastFound]; // The closest point of the simplex "v" is the last added point
return true;
}
// The algorithm failed to found a point
return false;
}
// Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3& v) {
decimal minDistSquare = DECIMAL_LARGEST;
Bits bit;
for (bit = mAllBits; bit != 0x0; bit--) {
if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
Vector3 u = computeClosestPointForSubset(bit);
decimal distSquare = u.dot(u);
if (distSquare < minDistSquare) {
minDistSquare = distSquare;
mBitsCurrentSimplex = bit;
v = u;
}
}
}
}
// Return the closest point "v" in the convex hull of the points in the subset
// represented by the bits "subset"
Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
mMaxLengthSquare = 0.0;
decimal deltaX = 0.0; // deltaX = sum of all det[subset][i]
int i;
Bits bit;
// For each four point in the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
// If the current point is in the subset
if (overlap(subset, bit)) {
// deltaX = sum of all det[subset][i]
deltaX += mDet[subset][i];
if (mMaxLengthSquare < mPointsLengthSquare[i]) {
mMaxLengthSquare = mPointsLengthSquare[i];
}
// Closest point v = sum(lambda_i * points[i])
v += mDet[subset][i] * mPoints[i];
}
}
assert(deltaX > 0.0);
// Return the closet point "v" in the convex hull for the given subset
return (decimal(1.0) / deltaX) * v;
}

189
src/collision/narrowphase/GJK/Simplex.h
View File

@ -1,189 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SIMPLEX_H
#define REACTPHYSICS3D_SIMPLEX_H
// Libraries
#include "mathematics/mathematics.h"
#include <vector>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Type definitions
typedef unsigned int Bits;
// Class Simplex
/**
* This class represents a simplex which is a set of 3D points. This
* class is used in the GJK algorithm. This implementation is based on
* the implementation discussed in the book "Collision Detection in 3D
* Environments". This class implements the Johnson's algorithm for
* computing the point of a simplex that is closest to the origin and also
* the smallest simplex needed to represent that closest point.
*/
class Simplex {
private:
// -------------------- Attributes -------------------- //
/// Current points
Vector3 mPoints[4];
/// pointsLengthSquare[i] = (points[i].length)^2
decimal mPointsLengthSquare[4];
/// Maximum length of pointsLengthSquare[i]
decimal mMaxLengthSquare;
/// Support points of object A in local coordinates
Vector3 mSuppPointsA[4];
/// Support points of object B in local coordinates
Vector3 mSuppPointsB[4];
/// diff[i][j] contains points[i] - points[j]
Vector3 mDiffLength[4][4];
/// Cached determinant values
decimal mDet[16][4];
/// norm[i][j] = (diff[i][j].length())^2
decimal mNormSquare[4][4];
/// 4 bits that identify the current points of the simplex
/// For instance, 0101 means that points[1] and points[3] are in the simplex
Bits mBitsCurrentSimplex;
/// Number between 1 and 4 that identify the last found support point
Bits mLastFound;
/// Position of the last found support point (lastFoundBit = 0x1 << lastFound)
Bits mLastFoundBit;
/// allBits = bitsCurrentSimplex | lastFoundBit;
Bits mAllBits;
// -------------------- Methods -------------------- //
/// Private copy-constructor
Simplex(const Simplex& simplex);
/// Private assignment operator
Simplex& operator=(const Simplex& simplex);
/// Return true if some bits of "a" overlap with bits of "b"
bool overlap(Bits a, Bits b) const;
/// Return true if the bits of "b" is a subset of the bits of "a"
bool isSubset(Bits a, Bits b) const;
/// Return true if the subset is a valid one for the closest point computation.
bool isValidSubset(Bits subset) const;
/// Return true if the subset is a proper subset.
bool isProperSubset(Bits subset) const;
/// Update the cached values used during the GJK algorithm
void updateCache();
/// Compute the cached determinant values
void computeDeterminants();
/// Return the closest point "v" in the convex hull of a subset of points
Vector3 computeClosestPointForSubset(Bits subset);
public:
// -------------------- Methods -------------------- //
/// Constructor
Simplex();
/// Destructor
~Simplex();
/// Return true if the simplex contains 4 points
bool isFull() const;
/// Return true if the simplex is empty
bool isEmpty() const;
/// Return the points of the simplex
unsigned int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB,
Vector3* mPoints) const;
/// Return the maximum squared length of a point
decimal getMaxLengthSquareOfAPoint() const;
/// Add a new support point of (A-B) into the simplex.
void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB);
/// Return true if the point is in the simplex
bool isPointInSimplex(const Vector3& point) const;
/// Return true if the set is affinely dependent
bool isAffinelyDependent() const;
/// Backup the closest point
void backupClosestPointInSimplex(Vector3& point);
/// Compute the closest points "pA" and "pB" of object A and B.
void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const;
/// Compute the closest point to the origin of the current simplex.
bool computeClosestPoint(Vector3& v);
};
// Return true if some bits of "a" overlap with bits of "b"
inline bool Simplex::overlap(Bits a, Bits b) const {
return ((a & b) != 0x0);
}
// Return true if the bits of "b" is a subset of the bits of "a"
inline bool Simplex::isSubset(Bits a, Bits b) const {
return ((a & b) == a);
}
// Return true if the simplex contains 4 points
inline bool Simplex::isFull() const {
return (mBitsCurrentSimplex == 0xf);
}
// Return true if the simple is empty
inline bool Simplex::isEmpty() const {
return (mBitsCurrentSimplex == 0x0);
}
// Return the maximum squared length of a point
inline decimal Simplex::getMaxLengthSquareOfAPoint() const {
return mMaxLengthSquare;
}
}
#endif

631
src/collision/narrowphase/GJK/VoronoiSimplex.cpp Normal file
View File

@ -0,0 +1,631 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "VoronoiSimplex.h"
#include "mathematics/Vector2.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
decimal VoronoiSimplex::epsilon = decimal(0.0001);
// Constructor
VoronoiSimplex::VoronoiSimplex() : mNbPoints(0), mRecomputeClosestPoint(false), mIsClosestPointValid(false) {
}
// Destructor
VoronoiSimplex::~VoronoiSimplex() {
}
// Add a new support point of (A-B) into the simplex
/// suppPointA : support point of object A in a direction -v
/// suppPointB : support point of object B in a direction v
/// point : support point of object (A-B) => point = suppPointA - suppPointB
void VoronoiSimplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) {
assert(!isFull());
mPoints[mNbPoints] = point;
mSuppPointsA[mNbPoints] = suppPointA;
mSuppPointsB[mNbPoints] = suppPointB;
mNbPoints++;
mRecomputeClosestPoint = true;
}
// Remove a point from the simplex
void VoronoiSimplex::removePoint(int index) {
assert(mNbPoints > 0);
mNbPoints--;
mPoints[index] = mPoints[mNbPoints];
mSuppPointsA[index] = mSuppPointsA[mNbPoints];
mSuppPointsB[index] = mSuppPointsB[mNbPoints];
}
// Reduce the simplex (only keep vertices that participate to the point closest to the origin)
/// bitsUsedPoints is seen as a sequence of bits representing whether the four points of
/// the simplex are used or not to represent the current closest point to the origin.
/// - The most right bit is set to one if the first point is used
/// - The second most right bit is set to one if the second point is used
/// - The third most right bit is set to one if the third point is used
/// - The fourth most right bit is set to one if the fourth point is used
void VoronoiSimplex::reduceSimplex(int bitsUsedPoints) {
if ((mNbPoints >= 4) && (bitsUsedPoints & 8) == 0)
removePoint(3);
if ((mNbPoints >= 3) && (bitsUsedPoints & 4) == 0)
removePoint(2);
if ((mNbPoints >= 2) && (bitsUsedPoints & 2) == 0)
removePoint(1);
if ((mNbPoints >= 1) && (bitsUsedPoints & 1) == 0)
removePoint(0);
}
// Return true if the point is in the simplex
bool VoronoiSimplex::isPointInSimplex(const Vector3& point) const {
// For each four possible points in the simplex
for (int i=0; i<mNbPoints; i++) {
// Compute the distance between the points
decimal distanceSquare = (mPoints[i] - point).lengthSquare();
// If the point is very close
if (distanceSquare <= epsilon) {
return true;
}
}
return false;
}
// Return the points of the simplex
int VoronoiSimplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB,
Vector3* points) const {
for (int i=0; i<mNbPoints; i++) {
points[i] = mPoints[i];
suppPointsA[i] = mSuppPointsA[i];
suppPointsB[i] = mSuppPointsB[i];
}
// Return the number of points in the simplex
return mNbPoints;
}
// Return true if the set is affinely dependent.
/// A set if affinely dependent if a point of the set
/// is an affine combination of other points in the set
bool VoronoiSimplex::isAffinelyDependent() const {
assert(mNbPoints >= 0 && mNbPoints <= 4);
switch(mNbPoints) {
case 0: return false;
case 1: return false;
// Two points are independent if there distance is larger than zero
case 2: return (mPoints[1] - mPoints[0]).lengthSquare() <= epsilon;
// Three points are independent if the triangle area is larger than zero
case 3: return (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]).lengthSquare() <= epsilon;
// Four points are independent if the tetrahedron volume is larger than zero
// Test in three different ways (for more robustness)
case 4: return std::abs((mPoints[1] - mPoints[0]).dot((mPoints[2] - mPoints[0]).cross(mPoints[3] - mPoints[0]))) <= epsilon;
}
return false;
}
// Compute the closest points "pA" and "pB" of object A and B.
/// The points are computed as follows :
/// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A
/// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B
/// with lambda_i = deltaX_i / deltaX
void VoronoiSimplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
pA = mClosestSuppPointA;
pB = mClosestSuppPointB;
}
// Recompute the closest point if the simplex has been modified
/// This method computes the point "v" of simplex that is closest to the origin.
/// The method returns true if a closest point has been found.
bool VoronoiSimplex::recomputeClosestPoint() {
assert(mNbPoints <= 4);
// If we need to recompute the closest point
if (mRecomputeClosestPoint) {
mRecomputeClosestPoint = false;
switch(mNbPoints) {
case 0:
// Cannot compute closest point when the simplex is empty
mIsClosestPointValid = false;
break;
case 1:
{
// There is a single point in the simplex, therefore, this point is
// the one closest to the origin
mClosestPoint = mPoints[0];
mClosestSuppPointA = mSuppPointsA[0];
mClosestSuppPointB = mSuppPointsB[0];
setBarycentricCoords(1, 0, 0, 0);
mIsClosestPointValid = checkClosestPointValid();
}
break;
case 2:
{
int bitsUsedPoints = 0;
float t;
// The simplex is a line AB (where A=mPoints[0] and B=mPoints[1].
// We need to find the point of that line closest to the origin
computeClosestPointOnSegment(mPoints[0], mPoints[1], bitsUsedPoints, t);
// Compute the closest point
mClosestSuppPointA = mSuppPointsA[0] + t * (mSuppPointsA[1] - mSuppPointsA[0]);
mClosestSuppPointB = mSuppPointsB[0] + t * (mSuppPointsB[1] - mSuppPointsB[0]);
mClosestPoint = mClosestSuppPointA - mClosestSuppPointB;
setBarycentricCoords(decimal(1.0) - t, t, 0, 0);
mIsClosestPointValid = checkClosestPointValid();
// Reduce the simplex (remove vertices that are not participating to
// the closest point
reduceSimplex(bitsUsedPoints);
}
break;
case 3:
{
// The simplex is a triangle. We need to find the point of that
// triangle that is closest to the origin
int bitsUsedVertices = 0;
Vector3 baryCoords;
// Compute the point of the triangle closest to the origin
computeClosestPointOnTriangle(mPoints[0], mPoints[1], mPoints[2], bitsUsedVertices, baryCoords);
mClosestSuppPointA = baryCoords[0] * mSuppPointsA[0] + baryCoords[1] * mSuppPointsA[1] +
baryCoords[2] * mSuppPointsA[2];
mClosestSuppPointB = baryCoords[0] * mSuppPointsB[0] + baryCoords[1] * mSuppPointsB[1] +
baryCoords[2] * mSuppPointsB[2];
mClosestPoint = mClosestSuppPointA - mClosestSuppPointB;
setBarycentricCoords(baryCoords.x, baryCoords.y, baryCoords.z, 0.0);
mIsClosestPointValid = checkClosestPointValid();
// Reduce the simplex (remove vertices that are not participating to
// the closest point
reduceSimplex(bitsUsedVertices);
}
break;
case 4:
{
// The simplex is a tetrahedron. We need to find the point of that
// tetrahedron that is closest to the origin
int bitsUsedVertices = 0;
Vector2 baryCoordsAB;
Vector2 baryCoordsCD;
bool isDegenerate;
// Compute the point closest to the origin on the tetrahedron
bool isOutside = computeClosestPointOnTetrahedron(mPoints[0], mPoints[1], mPoints[2], mPoints[3],
bitsUsedVertices, baryCoordsAB, baryCoordsCD, isDegenerate);
// If the origin is outside the tetrahedron
if (isOutside) {
// Compute the point of the tetrahedron closest to the origin
mClosestSuppPointA = baryCoordsAB.x * mSuppPointsA[0] + baryCoordsAB.y * mSuppPointsA[1] +
baryCoordsCD.x * mSuppPointsA[2] + baryCoordsCD.y * mSuppPointsA[3];
mClosestSuppPointB = baryCoordsAB.x * mSuppPointsB[0] + baryCoordsAB.y * mSuppPointsB[1] +
baryCoordsCD.x * mSuppPointsB[2] + baryCoordsCD.y * mSuppPointsB[3];
mClosestPoint = mClosestSuppPointA - mClosestSuppPointB;
setBarycentricCoords(baryCoordsAB.x, baryCoordsAB.y, baryCoordsCD.x, baryCoordsCD.y);
// Reduce the simplex (remove vertices that are not participating to
// the closest point
reduceSimplex(bitsUsedVertices);
}
else {
// If it is a degenerate case
if (isDegenerate) {
mIsClosestPointValid = false;
}
else {
// The origin is inside the tetrahedron, therefore, the closest point
// is the origin
setBarycentricCoords(0.0, 0.0, 0.0, 0.0);
mClosestSuppPointA.setToZero();
mClosestSuppPointB.setToZero();
mClosestPoint.setToZero();
mIsClosestPointValid = true;
}
break;
}
mIsClosestPointValid = checkClosestPointValid();
}
break;
}
}
return mIsClosestPointValid;
}
// Compute point of a line segment that is closest to the origin
void VoronoiSimplex::computeClosestPointOnSegment(const Vector3& a, const Vector3& b, int& bitUsedVertices,
float& t) const {
Vector3 AP = -a;
Vector3 AB = b - a;
decimal APDotAB = AP.dot(AB);
// If the closest point is on the side of A in the direction of B
if (APDotAB > decimal(0.0)) {
decimal lengthABSquare = AB.lengthSquare();
// If the closest point is on the segment AB
if (APDotAB < lengthABSquare) {
t = APDotAB / lengthABSquare;
bitUsedVertices = 3; // 0011 (both A and B are used)
}
else { // If the origin is on the side of B that is not in the direction of A
// Therefore, the closest point is B
t = decimal(1.0);
bitUsedVertices = 2; // 0010 (only B is used)
}
}
else { // If the origin is on the side of A that is not in the direction of B
// Therefore, the closest point of the line is A
t = decimal(0.0);
bitUsedVertices = 1; // 0001 (only A is used)
}
}
// Compute point on a triangle that is closest to the origin
/// This implementation is based on the one in the book
/// "Real-Time Collision Detection" by Christer Ericson.
void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
int& bitsUsedVertices, Vector3& baryCoordsABC) const {
// Check if the origin is in the Voronoi region of vertex A
Vector3 ab = b - a;
Vector3 ac = c - a;
Vector3 ap = -a;
decimal d1 = ab.dot(ap);
decimal d2 = ac.dot(ap);
if (d1 <= decimal(0.0) && d2 <= decimal(0.0)) {
// The origin is in the Voronoi region of vertex A
// Set the barycentric coords of the closest point on the triangle
baryCoordsABC.setAllValues(1.0, 0, 0);
bitsUsedVertices = 1; // 0001 (only A is used)
return;
}
// Check if the origin is in the Voronoi region of vertex B
Vector3 bp = -b;
decimal d3 = ab.dot(bp);
decimal d4 = ac.dot(bp);
if (d3 >= decimal(0.0) && d4 <= d3) {
// The origin is in the Voronoi region of vertex B
// Set the barycentric coords of the closest point on the triangle
baryCoordsABC.setAllValues(0.0, 1.0, 0);
bitsUsedVertices = 2; // 0010 (only B is used)
return;
}
// Check if the origin is in the Voronoi region of edge AB
decimal vc = d1 * d4 - d3 * d2;
if (vc <= decimal(0.0) && d1 >= decimal(0.0) && d3 <= decimal(0.0)) {
// The origin is in the Voronoi region of edge AB
// We return the projection of the origin on the edge AB
assert(std::abs(d1 - d3) > MACHINE_EPSILON);
decimal v = d1 / (d1 - d3);
// Set the barycentric coords of the closest point on the triangle
baryCoordsABC.setAllValues(decimal(1.0) - v, v, 0);
bitsUsedVertices = 3; // 0011 (A and B are used)
return;
}
// Check if the origin is in the Voronoi region of vertex C
Vector3 cp = -c;
decimal d5 = ab.dot(cp);
decimal d6 = ac.dot(cp);
if (d6 >= decimal(0.0) && d5 <= d6) {
// The origin is in the Voronoi region of vertex C
// Set the barycentric coords of the closest point on the triangle
baryCoordsABC.setAllValues(0.0, 0.0, 1.0);
bitsUsedVertices = 4; // 0100 (only C is used)
return;
}
// Check if the origin is in the Voronoi region of edge AC
decimal vb = d5 * d2 - d1 * d6;
if (vb <= decimal(0.0) && d2 >= decimal(0.0) && d6 <= decimal(0.0)) {
// The origin is in the Voronoi region of edge AC
// We return the projection of the origin on the edge AC
assert(std::abs(d2 - d6) > MACHINE_EPSILON);
decimal w = d2 / (d2 - d6);
// Set the barycentric coords of the closest point on the triangle
baryCoordsABC.setAllValues(decimal(1.0) - w, 0, w);
bitsUsedVertices = 5; // 0101 (A and C are used)
return;
}
// Check if the origin is in the Voronoi region of edge BC
decimal va = d3 * d6 - d5 * d4;
if (va <= decimal(0.0) && (d4 - d3) >= decimal(0.0) && (d5 - d6) >= decimal(0.0)) {
// The origin is in the Voronoi region of edge BC
// We return the projection of the origin on the edge BC
assert(std::abs((d4 - d3) + (d5 - d6)) > MACHINE_EPSILON);
decimal w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
// Set the barycentric coords of the closest point on the triangle
baryCoordsABC.setAllValues(0.0, decimal(1.0) - w, w);
bitsUsedVertices = 6; // 0110 (B and C are used)
return;
}
// The origin is in the Voronoi region of the face ABC
decimal denom = decimal(1.0) / (va + vb + vc);
decimal v = vb * denom;
decimal w = vc * denom;
// Set the barycentric coords of the closest point on the triangle
baryCoordsABC.setAllValues(1 - v - w, v, w);
bitsUsedVertices = 7; // 0111 (A, B and C are used)
return;
}
// Compute point of a tetrahedron that is closest to the origin
/// This implementation is based on the one in the book
/// "Real-Time Collision Detection" by Christer Ericson.
/// This method returns true if the origin is outside the tetrahedron.
bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Vector3& b, const Vector3& c,
const Vector3& d, int& bitsUsedPoints, Vector2& baryCoordsAB,
Vector2& baryCoordsCD, bool& isDegenerate) const {
isDegenerate = false;
// Start as if the origin was inside the tetrahedron
bitsUsedPoints = 15; // 1111 (A, B, C and D are used)
baryCoordsAB.setToZero();
baryCoordsCD.setToZero();
// Check if the origin is outside each tetrahedron face
int isOriginOutsideFaceABC = testOriginOutsideOfPlane(a, b, c, d);
int isOriginOutsideFaceACD = testOriginOutsideOfPlane(a, c, d, b);
int isOriginOutsideFaceADB = testOriginOutsideOfPlane(a, d, b, c);
int isOriginOutsideFaceBDC = testOriginOutsideOfPlane(b, d, c, a);
// If we have a degenerate tetrahedron
if (isOriginOutsideFaceABC < 0 || isOriginOutsideFaceACD < 0 ||
isOriginOutsideFaceADB < 0 || isOriginOutsideFaceBDC < 0) {
// The tetrahedron is degenerate
isDegenerate = true;
return false;
}
if (isOriginOutsideFaceABC == 0 && isOriginOutsideFaceACD == 0 &&
isOriginOutsideFaceADB == 0 && isOriginOutsideFaceBDC == 0) {
// The origin is inside the tetrahedron
return true;
}
// We know that the origin is outside the tetrahedron, we now need to find
// which of the four triangle faces is closest to it.
decimal closestSquareDistance = DECIMAL_LARGEST;
int tempUsedVertices;
Vector3 triangleBaryCoords;
// If the origin is outside face ABC
if (isOriginOutsideFaceABC) {
// Compute the closest point on this triangle face
computeClosestPointOnTriangle(a, b, c, tempUsedVertices, triangleBaryCoords);
Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * b +
triangleBaryCoords[2] * c;
decimal squareDist = closestPoint.lengthSquare();
// If the point on that face is the closest to the origin so far
if (squareDist < closestSquareDistance) {
// Use it as the current closest point
closestSquareDistance = squareDist;
baryCoordsAB.setAllValues(triangleBaryCoords[0], triangleBaryCoords[1]);
baryCoordsCD.setAllValues(triangleBaryCoords[2], 0.0);
bitsUsedPoints = tempUsedVertices;
}
}
// If the origin is outside face ACD
if (isOriginOutsideFaceACD) {
// Compute the closest point on this triangle face
computeClosestPointOnTriangle(a, c, d, tempUsedVertices, triangleBaryCoords);
Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * c +
triangleBaryCoords[2] * d;
decimal squareDist = closestPoint.lengthSquare();
// If the point on that face is the closest to the origin so far
if (squareDist < closestSquareDistance) {
// Use it as the current closest point
closestSquareDistance = squareDist;
baryCoordsAB.setAllValues(triangleBaryCoords[0], 0.0);
baryCoordsCD.setAllValues(triangleBaryCoords[1], triangleBaryCoords[2]);
bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 0, 2, 3);
}
}
// If the origin is outside face
if (isOriginOutsideFaceADB) {
// Compute the closest point on this triangle face
computeClosestPointOnTriangle(a, d, b, tempUsedVertices, triangleBaryCoords);
Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * d +
triangleBaryCoords[2] * b;
decimal squareDist = closestPoint.lengthSquare();
// If the point on that face is the closest to the origin so far
if (squareDist < closestSquareDistance) {
// Use it as the current closest point
closestSquareDistance = squareDist;
baryCoordsAB.setAllValues(triangleBaryCoords[0], triangleBaryCoords[2]);
baryCoordsCD.setAllValues(0.0, triangleBaryCoords[1]);
bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 0, 3, 1);
}
}
// If the origin is outside face
if (isOriginOutsideFaceBDC) {
// Compute the closest point on this triangle face
computeClosestPointOnTriangle(b, d, c, tempUsedVertices, triangleBaryCoords);
Vector3 closestPoint = triangleBaryCoords[0] * b + triangleBaryCoords[1] * d +
triangleBaryCoords[2] * c;
decimal squareDist = closestPoint.lengthSquare();
// If the point on that face is the closest to the origin so far
if (squareDist < closestSquareDistance) {
// Use it as the current closest point
baryCoordsAB.setAllValues(0.0, triangleBaryCoords[0]);
baryCoordsCD.setAllValues(triangleBaryCoords[2], triangleBaryCoords[1]);
bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 1, 3, 2);
}
}
return true;
}
// 0111 (1,2,3) => 0111
// 0011 (2,1,3) =>
int VoronoiSimplex::mapTriangleUsedVerticesToTetrahedron(int triangleUsedVertices, int first, int second, int third) const {
assert(triangleUsedVertices <= 7);
int tetrahedronUsedVertices = (((1 & triangleUsedVertices) != 0) << first) |
(((2 & triangleUsedVertices) != 0) << second) |
(((4 & triangleUsedVertices) != 0) << third);
assert(tetrahedronUsedVertices <= 14);
return tetrahedronUsedVertices;
}
// Test if a point is outside of the plane given by the points of the triangle (a, b, c)
/// This method returns 1 if the point d and the origin are on opposite sides of
/// the triangle face (a, b, c). It returns 0 if they are on the same side.
/// This implementation is based on the one in the book
/// "Real-Time Collision Detection" by Christer Ericson.
int VoronoiSimplex::testOriginOutsideOfPlane(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const {
// Normal of (a,b,c) triangle
const Vector3 n = (b-a).cross(c-a);
decimal signp = (-a).dot(n);
decimal signd = (d-a).dot(n);
// If the tetrahedron is degenerate (all points on the same plane)
// This should never happen because after a point is added into the simplex, the user
// of this class must check that the simplex is not affinely dependent using the
// isAffinelyDependent() method
if(signd * signd < epsilon * epsilon) {
return -1;
}
return signp * signd < decimal(0.0);
}
// Backup the closest point
void VoronoiSimplex::backupClosestPointInSimplex(Vector3& v) {
v = mClosestPoint;
}
// Return the maximum squared length of a point
decimal VoronoiSimplex::getMaxLengthSquareOfAPoint() const {
decimal maxLengthSquare = decimal(0.0);
for (int i=0; i<mNbPoints; i++) {
decimal lengthSquare = mPoints[i].lengthSquare();
if (lengthSquare > maxLengthSquare) {
maxLengthSquare = lengthSquare;
}
}
// Return the maximum squared length
return maxLengthSquare;
}

207
src/collision/narrowphase/GJK/VoronoiSimplex.h Normal file
View File

@ -0,0 +1,207 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_VORONOI_SIMPLEX_H
#define REACTPHYSICS3D_VORONOI_SIMPLEX_H
// Libraries
#include "mathematics/Vector3.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Type definitions
typedef unsigned int Bits;
// Class VoronoiSimplex
/**
* This class represents a simplex which is a set of 3D points. This
* class is used in the GJK algorithm. This implementation is based in the book
* "Real-Time Collision Detection" by Christer Ericson. This simple is used to
* replace theJohnson's algorithm for computing the point of a simplex that
* is closest to the origin and also the smallest simplex needed to represent
* that closest point.
*/
class VoronoiSimplex {
private:
// -------------------- Attributes -------------------- //
/// Current points
Vector3 mPoints[4];
/// Number of vertices in the simplex
int mNbPoints;
/// Barycentric coordinates of the closest point using simplex vertices
decimal mBarycentricCoords[4];
/// pointsLengthSquare[i] = (points[i].length)^2
decimal mPointsLengthSquare[4];
/// Support points of object A in local coordinates
Vector3 mSuppPointsA[4];
/// Support points of object B in local coordinates
Vector3 mSuppPointsB[4];
/// True if the closest point has to be recomputed (because the simplex has changed)
bool mRecomputeClosestPoint;
/// Current point that is closest to the origin
Vector3 mClosestPoint;
/// Current closest point on object A
Vector3 mClosestSuppPointA;
/// Current closest point on object B
Vector3 mClosestSuppPointB;
/// True if the last computed closest point is valid
bool mIsClosestPointValid;
/// Epsilon value
static decimal epsilon;
// -------------------- Methods -------------------- //
/// Private copy-constructor
VoronoiSimplex(const VoronoiSimplex& simplex);
/// Private assignment operator
VoronoiSimplex& operator=(const VoronoiSimplex& simplex);
/// Set the barycentric coordinates of the closest point
void setBarycentricCoords(decimal a, decimal b, decimal c, decimal d);
/// Recompute the closest point if the simplex has been modified
bool recomputeClosestPoint();
/// Return true if the
bool checkClosestPointValid() const;
/// Compute point of a line segment that is closest to the origin
void computeClosestPointOnSegment(const Vector3& a, const Vector3& b, int& bitUsedVertices,
float& t) const;
/// Compute point of a triangle that is closest to the origin
void computeClosestPointOnTriangle(const Vector3& a, const Vector3& b,
const Vector3& c, int& bitsUsedPoints, Vector3& baryCoordsABC) const;
/// Compute point of a tetrahedron that is closest to the origin
bool computeClosestPointOnTetrahedron(const Vector3& a, const Vector3& b,
const Vector3& c, const Vector3& d,
int& bitsUsedPoints, Vector2& baryCoordsAB, Vector2& baryCoordsCD,
bool& isDegenerate) const;
/// Test if a point is outside of the plane given by the points of the triangle (a, b, c)
int testOriginOutsideOfPlane(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const;
/// Remap the used vertices bits of a triangle to the corresponding used vertices of a tetrahedron
int mapTriangleUsedVerticesToTetrahedron(int triangleUsedVertices, int first, int second, int third) const;
public:
// -------------------- Methods -------------------- //
/// Constructor
VoronoiSimplex();
/// Destructor
~VoronoiSimplex();
/// Return true if the simplex contains 4 points
bool isFull() const;
/// Return true if the simplex is empty
bool isEmpty() const;
/// Return the points of the simplex
int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB, Vector3* mPoints) const;
/// Return the maximum squared length of a point
decimal getMaxLengthSquareOfAPoint() const;
/// Add a new support point of (A-B) into the simplex
void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB);
/// Remove a point from the simplex
void removePoint(int index);
/// Reduce the simplex (only keep vertices that participate to the point closest to the origin)
void reduceSimplex(int bitsUsedPoints);
/// Return true if the point is in the simplex
bool isPointInSimplex(const Vector3& point) const;
/// Return true if the set is affinely dependent
bool isAffinelyDependent() const;
/// Backup the closest point
void backupClosestPointInSimplex(Vector3& point);
/// Compute the closest points "pA" and "pB" of object A and B.
void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const;
/// Compute the closest point to the origin of the current simplex.
bool computeClosestPoint(Vector3& v);
};
// Return true if the simplex contains 4 points
inline bool VoronoiSimplex::isFull() const {
return mNbPoints == 4;
}
// Return true if the simple is empty
inline bool VoronoiSimplex::isEmpty() const {
return mNbPoints == 0;
}
// Set the barycentric coordinates of the closest point
inline void VoronoiSimplex::setBarycentricCoords(decimal a, decimal b, decimal c, decimal d) {
mBarycentricCoords[0] = a;
mBarycentricCoords[1] = b;
mBarycentricCoords[2] = c;
mBarycentricCoords[3] = d;
}
// Compute the closest point "v" to the origin of the current simplex.
inline bool VoronoiSimplex::computeClosestPoint(Vector3& v) {
bool isValid = recomputeClosestPoint();
v = mClosestPoint;
return isValid;
}
// Return true if the
inline bool VoronoiSimplex::checkClosestPointValid() const {
return mBarycentricCoords[0] >= decimal(0.0) && mBarycentricCoords[1] >= decimal(0.0) &&
mBarycentricCoords[2] >= decimal(0.0) && mBarycentricCoords[3] >= decimal(0.0);
}
}
#endif

76
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,16 +27,19 @@
#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
#include "engine/OverlappingPair.h"
#include "collision/CollisionShapeInfo.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
class CollisionDetection;
class Body;
class ContactManifoldInfo;
class PoolAllocator;
class OverlappingPair;
struct NarrowPhaseInfo;
struct ContactPointInfo;
class Profiler;
class MemoryAllocator;
// Class NarrowPhaseCallback
/**
@ -47,6 +50,8 @@ class NarrowPhaseCallback {
public:
virtual ~NarrowPhaseCallback() = default;
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo)=0;
@ -65,49 +70,50 @@ class NarrowPhaseAlgorithm {
// -------------------- Attributes -------------------- //
/// Pointer to the collision detection object
CollisionDetection* mCollisionDetection;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the memory allocator
MemoryAllocator* mMemoryAllocator;
/// Pointer to the profiler
Profiler* mProfiler;
/// Overlapping pair of the bodies currently tested for collision
OverlappingPair* mCurrentOverlappingPair;
// -------------------- Methods -------------------- //
/// Private copy-constructor
NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm);
/// Private assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm);
#endif
public :
// -------------------- Methods -------------------- //
/// Constructor
NarrowPhaseAlgorithm();
NarrowPhaseAlgorithm() = default;
/// Destructor
virtual ~NarrowPhaseAlgorithm();
virtual ~NarrowPhaseAlgorithm() = default;
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator);
/// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
/// Deleted copy-constructor
NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm) = delete;
/// Deleted assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volumes collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator)=0;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback)=0;
};
// Set the current overlapping pair of bodies
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
mCurrentOverlappingPair = overlappingPair;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void NarrowPhaseAlgorithm::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}

1051
src/collision/narrowphase/SAT/SATAlgorithm.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

180
src/collision/narrowphase/SAT/SATAlgorithm.h Normal file
View File

@ -0,0 +1,180 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SAT_ALGORITHM_H
#define REACTPHYSICS3D_SAT_ALGORITHM_H
// Libraries
#include "decimal.h"
#include "collision/HalfEdgeStructure.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class CapsuleShape;
class SphereShape;
class ContactManifoldInfo;
struct NarrowPhaseInfo;
class ConvexPolyhedronShape;
class MemoryAllocator;
class Profiler;
// Class SATAlgorithm
/**
* This class implements the Separating Axis Theorem algorithm (SAT).
* This algorithm is used to find the axis of minimum penetration between two convex polyhedra.
* If none is found, the objects are separated. Otherwise, the two objects are
* in contact and we use clipping to get the contact points.
*/
class SATAlgorithm {
private :
// -------------------- Attributes -------------------- //
/// Bias used to make sure the SAT algorithm returns the same penetration axis between frames
/// when there are multiple separating axis with the same penetration depth. The goal is to
/// make sure the contact manifold does not change too much between frames.
static const decimal SAME_SEPARATING_AXIS_BIAS;
/// Memory allocator
MemoryAllocator& mMemoryAllocator;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis)
bool testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1,
const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2,
const Transform& polyhedron1ToPolyhedron2) const;
/// Return true if the arcs AB and CD on the Gauss Map intersect
bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b,
const Vector3& c, const Vector3& d,
const Vector3& bCrossA, const Vector3& dCrossC) const;
/// Compute and return the distance between the two edges in the direction of the candidate separating axis
decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid,
const Vector3& edge1Direction, const Vector3& edge2Direction,
Vector3& outSeparatingAxis) const;
/// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2,
uint faceIndex) const;
/// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
/// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction
decimal computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron,
const SphereShape* sphere, const Vector3& sphereCenter) const;
/// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction
decimal computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron,
const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform,
Vector3& outFaceNormalCapsuleSpace) const;
/// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment
decimal computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule,
const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1,
const Vector3& edgeDirectionCapsuleSpace,
const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const;
/// Compute the contact points between two faces of two convex polyhedra.
bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2,
const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex,
NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const;
public :
// -------------------- Methods -------------------- //
/// Constructor
SATAlgorithm(MemoryAllocator& memoryAllocator);
/// Destructor
~SATAlgorithm() = default;
/// Deleted copy-constructor
SATAlgorithm(const SATAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
/// Test collision between a sphere and a convex mesh
bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Test collision between a capsule and a convex mesh
bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const;
// This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference
bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
const Vector3& edgeAdjacentFace2Normal) const;
/// Test collision between two convex meshes
bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void SATAlgorithm::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

139
src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp Executable file
View File

@ -0,0 +1,139 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "SphereVsCapsuleAlgorithm.h"
#include "collision/shapes/SphereShape.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/NarrowPhaseInfo.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a capsule
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
// Get the transform from sphere local-space to capsule local-space
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse();
const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform;
// Transform the center of the sphere into the local-space of the capsule shape
const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
// Compute the end-points of the inner segment of the capsule
const decimal capsuleHalfHeight = capsuleShape->getHeight() * decimal(0.5);
const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0);
const Vector3 capsuleSegB(0, capsuleHalfHeight, 0);
// Compute the point on the inner capsule segment that is the closes to center of sphere
const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter);
// Compute the distance between the sphere center and the closest point on the segment
Vector3 sphereCenterToSegment = (closestPointOnSegment - sphereCenter);
const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare();
// Compute the sum of the radius of the sphere and the capsule (virtual sphere)
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
// If the collision shapes overlap
if (sphereSegmentDistanceSquare < sumRadius * sumRadius) {
decimal penetrationDepth;
Vector3 normalWorld;
Vector3 contactPointSphereLocal;
Vector3 contactPointCapsuleLocal;
if (reportContacts) {
// If the sphere center is not on the capsule inner segment
if (sphereSegmentDistanceSquare > MACHINE_EPSILON) {
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
sphereCenterToSegment /= sphereSegmentDistance;
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
penetrationDepth = sumRadius - sphereSegmentDistance;
if (!isSphereShape1) {
normalWorld = -normalWorld;
}
}
else { // If the sphere center is on the capsule inner segment (degenerate case)
// We take any direction that is orthogonal to the inner capsule segment as a contact normal
// Capsule inner segment
Vector3 capsuleSegment = (capsuleSegB - capsuleSegA).getUnit();
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(capsuleSegment.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(capsuleSegment.y); // abs(vec2.dot(seg2))
penetrationDepth = sumRadius;
// We choose as a contact normal, any direction that is perpendicular to the inner capsule segment
Vector3 normalCapsuleSpace = cosA1 < cosA2 ? capsuleSegment.cross(vec1) : capsuleSegment.cross(vec2);
normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace;
// Compute the two local contact points
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius());
contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius();
}
if (penetrationDepth <= decimal(0.0)) {
return false;
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
}
return true;
}
return false;
}

75
src/collision/narrowphase/SphereVsCapsuleAlgorithm.h Normal file
View File

@ -0,0 +1,75 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class Body;
class ContactPoint;
// Class SphereVsCapsuleAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between a sphere collision shape and a capsule collision shape.
* For this case, we do not use GJK or SAT algorithm. We directly compute the
* contact points and contact normal. This is based on the "Robust Contact
* Creation for Physics Simulation" presentation by Dirk Gregorius.
*/
class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
SphereVsCapsuleAlgorithm() = default;
/// Destructor
virtual ~SphereVsCapsuleAlgorithm() override = default;
/// Deleted copy-constructor
SphereVsCapsuleAlgorithm(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a capsule
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

91
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp Normal file
View File

@ -0,0 +1,91 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "SphereVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "collision/NarrowPhaseInfo.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
#ifdef IS_PROFILING_ACTIVE
gjkAlgorithm.setProfiler(mProfiler);
#endif
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
// Return true
return true;
}
// If we have overlap even without the margins (deep penetration)
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
satAlgorithm.setProfiler(mProfiler);
#endif
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
}
return false;
}

80
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h Normal file
View File

@ -0,0 +1,80 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class ContactPoint;
class Body;
// Class SphereVsConvexPolyhedronAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between a sphere and a convex polyhedron. The sphere is basically a
* point with a margin around it. The idea of this method is to use
* the GJK algorithm first. If GJK reports that the two objects are
* separated, we are done. If GJK reports that the objects are overlapping
* in the sphere margin, GJK will also report a contact point and a contact
* normal. However, if GJK reports that the object are in deep penetration
* (the center of the sphere is inside the polyhedron) we run the SAT
* algorithm to get the contact point and contact normal.
* This is based on the "Robust Contact Creation for Physics Simulation"
* presentation by Dirk Gregorius.
*/
class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
SphereVsConvexPolyhedronAlgorithm() = default;
/// Destructor
virtual ~SphereVsConvexPolyhedronAlgorithm() override = default;
/// Deleted copy-constructor
SphereVsConvexPolyhedronAlgorithm(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

171
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp Normal file → Executable file
View File

@ -1,80 +1,91 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "SphereVsSphereAlgorithm.h"
#include "collision/shapes/SphereShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() : NarrowPhaseAlgorithm() {
}
// Destructor
SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
}
void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
// Get the local-space to world-space transforms
const Transform& transform1 = shape1Info.shapeToWorldTransform;
const Transform& transform2 = shape2Info.shapeToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit();
Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
centerSphere1InBody2LocalSpace.getUnit();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
// Notify about the new contact
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
}
}
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "SphereVsSphereAlgorithm.h"
#include "collision/shapes/SphereShape.h"
#include "collision/NarrowPhaseInfo.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters < sumRadius * sumRadius) {
if (reportContacts) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
Vector3 intersectionOnBody1;
Vector3 intersectionOnBody2;
Vector3 normal;
// If the two sphere centers are not at the same position
if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit();
normal = vectorBetweenCenters.getUnit();
}
else { // If the sphere centers are at the same position (degenerate case)
// Take any contact normal direction
normal.setAllValues(0, 1, 0);
intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal);
intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal);
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
}
return true;
}
return false;
}

148
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -1,74 +1,74 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class SphereVsSphereAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two sphere collision shapes.
*/
class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
protected :
// -------------------- Methods -------------------- //
/// Private copy-constructor
SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm);
/// Private assignment operator
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm);
public :
// -------------------- Methods -------------------- //
/// Constructor
SphereVsSphereAlgorithm();
/// Destructor
virtual ~SphereVsSphereAlgorithm();
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback);
};
}
#endif
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class ContactPoint;
class Body;
// Class SphereVsSphereAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two sphere collision shapes. This algorithm finds the contact
* point and contact normal between two spheres if they are colliding.
* This case is simple, we do not need to use GJK or SAT algorithm. We
* directly compute the contact points if any.
*/
class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
SphereVsSphereAlgorithm() = default;
/// Destructor
virtual ~SphereVsSphereAlgorithm() override = default;
/// Deleted copy-constructor
SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

12
src/collision/shapes/AABB.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -31,11 +31,6 @@
using namespace reactphysics3d;
using namespace std;
// Constructor
AABB::AABB() {
}
// Constructor
AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
:mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) {
@ -48,11 +43,6 @@ AABB::AABB(const AABB& aabb)
}
// Destructor
AABB::~AABB() {
}
// Merge the AABB in parameter with the current one
void AABB::mergeWithAABB(const AABB& aabb) {
mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x);

6
src/collision/shapes/AABB.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -56,7 +56,7 @@ class AABB {
// -------------------- Methods -------------------- //
/// Constructor
AABB();
AABB() = default;
/// Constructor
AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
@ -65,7 +65,7 @@ class AABB {
AABB(const AABB& aabb);
/// Destructor
~AABB();
~AABB() = default;
/// Return the center point
Vector3 getCenter() const;

63
src/collision/shapes/BoxShape.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,7 +27,8 @@
#include "BoxShape.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
#include <vector>
#include "memory/MemoryManager.h"
#include "collision/RaycastInfo.h"
#include <cassert>
using namespace reactphysics3d;
@ -35,18 +36,49 @@ using namespace reactphysics3d;
// Constructor
/**
* @param extent The vector with the three extents of the box (in meters)
* @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const Vector3& extent, decimal margin)
: ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > decimal(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin);
}
BoxShape::BoxShape(const Vector3& extent)
: ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent),
mHalfEdgeStructure(MemoryManager::getBaseAllocator(), 6, 8, 24) {
// Destructor
BoxShape::~BoxShape() {
assert(extent.x > decimal(0.0));
assert(extent.y > decimal(0.0));
assert(extent.z > decimal(0.0));
// Vertices
mHalfEdgeStructure.addVertex(0);
mHalfEdgeStructure.addVertex(1);
mHalfEdgeStructure.addVertex(2);
mHalfEdgeStructure.addVertex(3);
mHalfEdgeStructure.addVertex(4);
mHalfEdgeStructure.addVertex(5);
mHalfEdgeStructure.addVertex(6);
mHalfEdgeStructure.addVertex(7);
MemoryAllocator& allocator = MemoryManager::getBaseAllocator();
// Faces
List<uint> face0(allocator, 4);
face0.add(0); face0.add(1); face0.add(2); face0.add(3);
List<uint> face1(allocator, 4);
face1.add(1); face1.add(5); face1.add(6); face1.add(2);
List<uint> face2(allocator, 4);
face2.add(4); face2.add(7); face2.add(6); face2.add(5);
List<uint> face3(allocator, 4);
face3.add(4); face3.add(0); face3.add(3); face3.add(7);
List<uint> face4(allocator, 4);
face4.add(4); face4.add(5); face4.add(1); face4.add(0);
List<uint> face5(allocator, 4);
face5.add(2); face5.add(6); face5.add(7); face5.add(3);
mHalfEdgeStructure.addFace(face0);
mHalfEdgeStructure.addFace(face1);
mHalfEdgeStructure.addFace(face2);
mHalfEdgeStructure.addFace(face3);
mHalfEdgeStructure.addFace(face4);
mHalfEdgeStructure.addFace(face5);
mHalfEdgeStructure.init();
}
// Return the local inertia tensor of the collision shape
@ -57,17 +89,16 @@ BoxShape::~BoxShape() {
*/
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
decimal xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z;
decimal xSquare = mExtent.x * mExtent.x;
decimal ySquare = mExtent.y * mExtent.y;
decimal zSquare = mExtent.z * mExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
}
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST;

191
src/collision/shapes/BoxShape.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,30 +27,24 @@
#define REACTPHYSICS3D_BOX_SHAPE_H
// Libraries
#include <cfloat>
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "ConvexPolyhedronShape.h"
#include "mathematics/mathematics.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class CollisionBody;
class DefaultAllocator;
// Class BoxShape
/**
* This class represents a 3D box shape. Those axis are unit length.
* The three extents are half-widths of the box along the three
* axis x, y, z local axis. The "transform" of the corresponding
* rigid body will give an orientation and a position to the box. This
* collision shape uses an extra margin distance around it for collision
* detection purpose. The default margin is 4cm (if your units are meters,
* which is recommended). In case, you want to simulate small objects
* (smaller than the margin distance), you might want to reduce the margin by
* specifying your own margin distance using the "margin" parameter in the
* constructor of the box shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
* body will give an orientation and a position to the box.
*/
class BoxShape : public ConvexShape {
class BoxShape : public ConvexPolyhedronShape {
protected :
@ -59,48 +53,77 @@ class BoxShape : public ConvexShape {
/// Extent sizes of the box in the x, y and z direction
Vector3 mExtent;
/// Half-edge structure of the polyhedron
HalfEdgeStructure mHalfEdgeStructure;
// -------------------- Methods -------------------- //
/// Private copy-constructor
BoxShape(const BoxShape& shape);
/// Private assignment operator
BoxShape& operator=(const BoxShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
virtual size_t getSizeInBytes() const override;
public :
// -------------------- Methods -------------------- //
/// Constructor
BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN);
BoxShape(const Vector3& extent);
/// Destructor
virtual ~BoxShape();
virtual ~BoxShape() override = default;
/// Deleted copy-constructor
BoxShape(const BoxShape& shape) = delete;
/// Deleted assignment operator
BoxShape& operator=(const BoxShape& shape) = delete;
/// Return the extents of the box
Vector3 getExtent() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
/// Return the number of faces of the polyhedron
virtual uint getNbFaces() const override;
/// Return a given face of the polyhedron
virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const override;
/// Return a given vertex of the polyhedron
virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override;
/// Return the number of half-edges of the polyhedron
virtual uint getNbHalfEdges() const override;
/// Return a given half-edge of the polyhedron
virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
/// Return the position of a given vertex
virtual Vector3 getVertexPosition(uint vertexIndex) const override;
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
/// Return the string representation of the shape
virtual std::string to_string() const override;
};
// Return the extents of the box
@ -108,15 +131,7 @@ class BoxShape : public ConvexShape {
* @return The vector with the three extents of the box shape (in meters)
*/
inline Vector3 BoxShape::getExtent() const {
return mExtent + Vector3(mMargin, mMargin, mMargin);
}
// Set the scaling vector of the collision shape
inline void BoxShape::setLocalScaling(const Vector3& scaling) {
mExtent = (mExtent / mScaling) * scaling;
CollisionShape::setLocalScaling(scaling);
return mExtent;
}
// Return the local bounds of the shape in x, y and z directions
@ -128,7 +143,7 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) {
inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds
max = mExtent + Vector3(mMargin, mMargin, mMargin);
max = mExtent;
// Minimum bounds
min = -max;
@ -139,13 +154,12 @@ inline size_t BoxShape::getSizeInBytes() const {
return sizeof(BoxShape);
}
// Return a local support point in a given direction without the objec margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Return a local support point in a given direction without the object margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
direction.y < 0.0 ? -mExtent.y : mExtent.y,
direction.z < 0.0 ? -mExtent.z : mExtent.z);
return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x,
direction.y < decimal(0.0) ? -mExtent.y : mExtent.y,
direction.z < decimal(0.0) ? -mExtent.z : mExtent.z);
}
// Return true if a point is inside the collision shape
@ -155,6 +169,87 @@ inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* pro
localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]);
}
// Return the number of faces of the polyhedron
inline uint BoxShape::getNbFaces() const {
return 6;
}
// Return a given face of the polyhedron
inline const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const {
assert(faceIndex < mHalfEdgeStructure.getNbFaces());
return mHalfEdgeStructure.getFace(faceIndex);
}
// Return the number of vertices of the polyhedron
inline uint BoxShape::getNbVertices() const {
return 8;
}
// Return a given vertex of the polyhedron
inline HalfEdgeStructure::Vertex BoxShape::getVertex(uint vertexIndex) const {
assert(vertexIndex < getNbVertices());
return mHalfEdgeStructure.getVertex(vertexIndex);
}
// Return the position of a given vertex
inline Vector3 BoxShape::getVertexPosition(uint vertexIndex) const {
assert(vertexIndex < getNbVertices());
Vector3 extent = getExtent();
switch(vertexIndex) {
case 0: return Vector3(-extent.x, -extent.y, extent.z);
case 1: return Vector3(extent.x, -extent.y, extent.z);
case 2: return Vector3(extent.x, extent.y, extent.z);
case 3: return Vector3(-extent.x, extent.y, extent.z);
case 4: return Vector3(-extent.x, -extent.y, -extent.z);
case 5: return Vector3(extent.x, -extent.y, -extent.z);
case 6: return Vector3(extent.x, extent.y, -extent.z);
case 7: return Vector3(-extent.x, extent.y, -extent.z);
}
assert(false);
return Vector3::zero();
}
// Return the normal vector of a given face of the polyhedron
inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
assert(faceIndex < getNbFaces());
switch(faceIndex) {
case 0: return Vector3(0, 0, 1);
case 1: return Vector3(1, 0, 0);
case 2: return Vector3(0, 0, -1);
case 3: return Vector3(-1, 0, 0);
case 4: return Vector3(0, -1, 0);
case 5: return Vector3(0, 1, 0);
}
assert(false);
return Vector3::zero();
}
// Return the centroid of the box
inline Vector3 BoxShape::getCentroid() const {
return Vector3::zero();
}
// Return the string representation of the shape
inline std::string BoxShape::to_string() const {
return "BoxShape{extents=" + mExtent.to_string() + "}";
}
// Return the number of half-edges of the polyhedron
inline uint BoxShape::getNbHalfEdges() const {
return 24;
}
// Return a given half-edge of the polyhedron
inline const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const {
assert(edgeIndex < getNbHalfEdges());
return mHalfEdgeStructure.getHalfEdge(edgeIndex);
}
}
#endif

12
src/collision/shapes/CapsuleShape.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -27,6 +27,7 @@
#include "CapsuleShape.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
#include "collision/RaycastInfo.h"
#include <cassert>
using namespace reactphysics3d;
@ -37,16 +38,11 @@ using namespace reactphysics3d;
* @param height The height of the capsule (in meters)
*/
CapsuleShape::CapsuleShape(decimal radius, decimal height)
: ConvexShape(CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
: ConvexShape(CollisionShapeName::CAPSULE, CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
}
// Destructor
CapsuleShape::~CapsuleShape() {
}
// Return the local inertia tensor of the capsule
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
@ -90,7 +86,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
}
// Raycast method with feedback information
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
const Vector3 n = ray.point2 - ray.point1;

64
src/collision/shapes/CapsuleShape.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -28,12 +28,14 @@
// Libraries
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class CollisionBody;
// Class CapsuleShape
/**
* This class represents a capsule collision shape that is defined around the Y axis.
@ -55,21 +57,14 @@ class CapsuleShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Private copy-constructor
CapsuleShape(const CapsuleShape& shape);
/// Private assignment operator
CapsuleShape& operator=(const CapsuleShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
@ -77,7 +72,7 @@ class CapsuleShape : public ConvexShape {
Vector3& hitLocalPoint, decimal& hitFraction) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
virtual size_t getSizeInBytes() const override;
public :
@ -87,7 +82,13 @@ class CapsuleShape : public ConvexShape {
CapsuleShape(decimal radius, decimal height);
/// Destructor
virtual ~CapsuleShape();
virtual ~CapsuleShape() override = default;
/// Deleted copy-constructor
CapsuleShape(const CapsuleShape& shape) = delete;
/// Deleted assignment operator
CapsuleShape& operator=(const CapsuleShape& shape) = delete;
/// Return the radius of the capsule
decimal getRadius() const;
@ -95,14 +96,17 @@ class CapsuleShape : public ConvexShape {
/// Return the height of the capsule
decimal getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
/// Return the string representation of the shape
virtual std::string to_string() const override;
};
// Get the radius of the capsule
@ -121,15 +125,6 @@ inline decimal CapsuleShape::getHeight() const {
return mHalfHeight + mHalfHeight;
}
// Set the scaling vector of the collision shape
inline void CapsuleShape::setLocalScaling(const Vector3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mMargin = (mMargin / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling);
}
// Return the number of bytes used by the collision shape
inline size_t CapsuleShape::getSizeInBytes() const {
return sizeof(CapsuleShape);
@ -154,6 +149,11 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
min.z = min.x;
}
// Return true if the collision shape is a polyhedron
inline bool CapsuleShape::isPolyhedron() const {
return false;
}
// Return a local support point in a given direction without the object margin.
/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d"
/// of the convex hull of a set of convex objects is the support point "p" in the set of all
@ -161,8 +161,7 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
/// Therefore, in this method, we compute the support points of both top and bottom spheres of
/// the capsule and return the point with the maximum dot product with the direction vector. Note
/// that the object margin is implicitly the radius and height of the capsule.
inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
// Support point top sphere
decimal dotProductTop = mHalfHeight * direction.y;
@ -179,6 +178,11 @@ inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& di
}
}
// Return the string representation of the shape
inline std::string CapsuleShape::to_string() const {
return "CapsuleShape{halfHeight=" + std::to_string(mHalfHeight) + ", radius=" + std::to_string(getRadius()) + "}";
}
}
#endif

63
src/collision/shapes/CollisionShape.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -25,51 +25,68 @@
// Libraries
#include "CollisionShape.h"
#include "engine/Profiler.h"
#include "utils/Profiler.h"
#include "body/CollisionBody.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) {
}
CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type)
: mType(type), mName(name), mId(0) {
// Destructor
CollisionShape::~CollisionShape() {
#ifdef IS_PROFILING_ACTIVE
mProfiler = nullptr;
#endif
}
// Compute the world-space AABB of the collision shape given a transform
// Compute the world-space AABB of the collision shape given a transform from shape
// local-space to world-space. The technique is described in the book Real-Time Collision
// Detection by Christer Ericson.
/**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape
* @param transform Transform from shape local-space to world-space used to compute
* the AABB of the collision shape
*/
void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
PROFILE("CollisionShape::computeAABB()");
RP3D_PROFILE("CollisionShape::computeAABB()", mProfiler);
// Get the local bounds in x,y and z direction
Vector3 minBounds;
Vector3 maxBounds;
getLocalBounds(minBounds, maxBounds);
// Rotate the local bounds according to the orientation of the body
Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
worldAxis.getColumn(1).dot(minBounds),
worldAxis.getColumn(2).dot(minBounds));
Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
worldAxis.getColumn(1).dot(maxBounds),
worldAxis.getColumn(2).dot(maxBounds));
const Vector3 translation = transform.getPosition();
Matrix3x3 matrix = transform.getOrientation().getMatrix();
Vector3 resultMin;
Vector3 resultMax;
// Compute the minimum and maximum coordinates of the rotated extents
Vector3 minCoordinates = transform.getPosition() + worldMinBounds;
Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds;
// For each of the three axis
for (int i=0; i<3; i++) {
// Add translation component
resultMin[i] = translation[i];
resultMax[i] = translation[i];
for (int j=0; j<3; j++) {
decimal e = matrix[i][j] * minBounds[j];
decimal f = matrix[i][j] * maxBounds[j];
if (e < f) {
resultMin[i] += e;
resultMax[i] += f;
}
else {
resultMin[i] += f;
resultMax[i] += e;
}
}
}
// Update the AABB with the new minimum and maximum coordinates
aabb.setMin(minCoordinates);
aabb.setMax(maxCoordinates);
aabb.setMin(resultMin);
aabb.setMax(resultMax);
}

132
src/collision/shapes/CollisionShape.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -28,21 +28,27 @@
// Libraries
#include <cassert>
#include <typeinfo>
#include "mathematics/Vector3.h"
#include "mathematics/Matrix3x3.h"
#include "mathematics/Ray.h"
#include "AABB.h"
#include "collision/RaycastInfo.h"
#include "memory/MemoryAllocator.h"
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class Profiler;
class PoolAllocator;
class AABB;
class Transform;
struct Ray;
struct RaycastInfo;
struct Vector3;
class Matrix3x3;
/// Type of the collision shape
enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
/// Type of collision shapes
enum class CollisionShapeType {SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_SHAPE};
const int NB_COLLISION_SHAPE_TYPES = 4;
/// Names of collision shapes
enum class CollisionShapeName { TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, TRIANGLE_MESH, HEIGHTFIELD };
// Declarations
class ProxyShape;
@ -62,22 +68,26 @@ class CollisionShape {
/// Type of the collision shape
CollisionShapeType mType;
/// Scaling vector of the collision shape
Vector3 mScaling;
/// Name of the colision shape
CollisionShapeName mName;
/// Unique identifier of the shape inside an overlapping pair
uint mId;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Private copy-constructor
CollisionShape(const CollisionShape& shape);
/// Private assignment operator
CollisionShape& operator=(const CollisionShape& shape);
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const=0;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0;
@ -87,25 +97,37 @@ class CollisionShape {
// -------------------- Methods -------------------- //
/// Constructor
CollisionShape(CollisionShapeType type);
CollisionShape(CollisionShapeName name, CollisionShapeType type);
/// Destructor
virtual ~CollisionShape();
virtual ~CollisionShape() = default;
/// Return the type of the collision shapes
/// Deleted copy-constructor
CollisionShape(const CollisionShape& shape) = delete;
/// Deleted assignment operator
CollisionShape& operator=(const CollisionShape& shape) = delete;
/// Return the name of the collision shape
CollisionShapeName getName() const;
/// Return the type of the collision shape
CollisionShapeType getType() const;
/// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const=0;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const=0;
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
/// Return the scaling vector of the collision shape
Vector3 getScaling() const;
Vector3 getLocalScaling() const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the id of the shape
uint getId() const;
/// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
@ -113,12 +135,15 @@ class CollisionShape {
/// Compute the world-space AABB of the collision shape given a transform
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
/// Return true if the collision shape type is a convex shape
static bool isConvex(CollisionShapeType shapeType);
/// Return the string representation of the shape
virtual std::string to_string() const=0;
/// Return the maximum number of contact manifolds in an overlapping pair given two shape types
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2);
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler);
#endif
// -------------------- Friendship -------------------- //
@ -126,41 +151,36 @@ class CollisionShape {
friend class CollisionWorld;
};
// Return the name of the collision shape
/**
* @return The name of the collision shape (box, sphere, triangle, ...)
*/
inline CollisionShapeName CollisionShape::getName() const {
return mName;
}
// Return the type of the collision shape
/**
* @return The type of the collision shape (box, sphere, cylinder, ...)
* @return The type of the collision shape (sphere, capsule, convex polyhedron, concave mesh)
*/
inline CollisionShapeType CollisionShape::getType() const {
return mType;
}
// Return true if the collision shape type is a convex shape
inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
return shapeType != CONCAVE_MESH && shapeType != HEIGHTFIELD;
// Return the id of the shape
inline uint CollisionShape::getId() const {
return mId;
}
// Return the scaling vector of the collision shape
inline Vector3 CollisionShape::getScaling() const {
return mScaling;
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void CollisionShape::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
// Set the scaling vector of the collision shape
inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling;
}
// Return the maximum number of contact manifolds allowed in an overlapping
// pair wit the given two collision shape types
inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2) {
// If both shapes are convex
if (isConvex(shapeType1) && isConvex(shapeType2)) {
return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
} // If there is at least one concave shape
else {
return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
}
}
#endif
}

244
src/collision/shapes/ConcaveMeshShape.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -25,24 +25,25 @@
// Libraries
#include "ConcaveMeshShape.h"
#include "memory/MemoryManager.h"
#include "collision/RaycastInfo.h"
#include "collision/TriangleMesh.h"
#include "utils/Profiler.h"
#include "collision/TriangleVertexArray.h"
using namespace reactphysics3d;
// Constructor
ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh)
: ConcaveShape(CONCAVE_MESH) {
ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh, const Vector3& scaling)
: ConcaveShape(CollisionShapeName::TRIANGLE_MESH), mDynamicAABBTree(MemoryManager::getBaseAllocator()),
mScaling(scaling) {
mTriangleMesh = triangleMesh;
mRaycastTestType = FRONT;
mRaycastTestType = TriangleRaycastSide::FRONT;
// Insert all the triangles into the dynamic AABB tree
initBVHTree();
}
// Destructor
ConcaveMeshShape::~ConcaveMeshShape() {
}
// Insert all the triangles into the dynamic AABB tree
void ConcaveMeshShape::initBVHTree() {
@ -54,55 +55,21 @@ void ConcaveMeshShape::initBVHTree() {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
// For each triangle of the concave mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
Vector3 trianglePoints[3];
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
// Get the triangle vertices
triangleVertexArray->getTriangleVertices(triangleIndex, trianglePoints);
// Get the index of the current vertex in the triangle
int vertexIndex = 0;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
}
else {
assert(false);
}
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
}
else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
}
else {
assert(false);
}
}
// Apply the scaling factor to the vertices
trianglePoints[0] *= mScaling.x;
trianglePoints[1] *= mScaling.y;
trianglePoints[2] *= mScaling.z;
// Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints);
aabb.inflate(mTriangleMargin, mTriangleMargin, mTriangleMargin);
// Add the AABB with the index of the triangle into the dynamic AABB tree
mDynamicAABBTree.addObject(aabb, subPart, triangleIndex);
@ -111,56 +78,32 @@ void ConcaveMeshShape::initBVHTree() {
}
// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
// given the start vertex index pointer of the triangle
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
Vector3* outTriangleVertices) const {
void ConcaveMeshShape::getTriangleVertices(uint subPart, uint triangleIndex,
Vector3* outTriangleVertices) const {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
// Get the vertices coordinates of the triangle
triangleVertexArray->getTriangleVertices(triangleIndex, outTriangleVertices);
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle
int vertexIndex = 0;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
}
else {
assert(false);
}
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
}
else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
}
else {
assert(false);
}
}
// Apply the scaling factor to the vertices
outTriangleVertices[0] *= mScaling.x;
outTriangleVertices[1] *= mScaling.y;
outTriangleVertices[2] *= mScaling.z;
}
// Return the three vertex normals (in the array outVerticesNormals) of a triangle
void ConcaveMeshShape::getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
// Get the vertices normals of the triangle
triangleVertexArray->getTriangleVerticesNormals(triangleIndex, outVerticesNormals);
}
// Use a callback method on all triangles of the concave shape inside a given AABB
void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
@ -174,12 +117,19 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
// Raycast method with feedback information
/// Note that only the first triangle hit by the ray in the mesh will be returned, even if
/// the ray hits many triangles.
bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
PROFILE("ConcaveMeshShape::raycast()");
RP3D_PROFILE("ConcaveMeshShape::raycast()", mProfiler);
// Create the callback object that will compute ray casting against triangles
ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray, allocator);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
raycastCallback.setProfiler(mProfiler);
#endif
// Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray.
// The raycastCallback object will then compute ray casting against the triangles
@ -191,11 +141,27 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
return raycastCallback.getIsHit();
}
// Compute the shape Id for a given triangle of the mesh
uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const {
uint shapeId = 0;
uint i=0;
while (i < subPart) {
shapeId += mTriangleMesh->getSubpart(i)->getNbTriangles();
i++;
}
return shapeId + triangleIndex;
}
// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
// Add the id of the hit AABB node into
mHitAABBNodes.push_back(nodeId);
mHitAABBNodes.add(nodeId);
return ray.maxFraction;
}
@ -203,7 +169,7 @@ decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const R
// Raycast all collision shapes that have been collected
void ConcaveMeshRaycastCallback::raycastTriangles() {
std::vector<int>::const_iterator it;
List<int>::Iterator it;
decimal smallestHitFraction = mRay.maxFraction;
for (it = mHitAABBNodes.begin(); it != mHitAABBNodes.end(); ++it) {
@ -213,16 +179,26 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints);
// Get the vertices normals of the triangle
Vector3 verticesNormals[3];
mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Create a triangle collision shape
decimal margin = mConcaveMeshShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mAllocator);
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
#ifdef IS_PROFILING_ACTIVE
// Set the profiler to the triangle shape
triangleShape.setProfiler(mProfiler);
#endif
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
@ -240,5 +216,71 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
smallestHitFraction = raycastInfo.hitFraction;
mIsHit = true;
}
}
}
// Return the string representation of the shape
std::string ConcaveMeshShape::to_string() const {
std::stringstream ss;
ss << "ConcaveMeshShape{" << std::endl;
ss << "nbSubparts=" << mTriangleMesh->getNbSubparts() << std::endl;
// Vertices array
for (uint subPart=0; subPart<mTriangleMesh->getNbSubparts(); subPart++) {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
ss << "subpart" << subPart << "={" << std::endl;
ss << "nbVertices=" << triangleVertexArray->getNbVertices() << std::endl;
ss << "nbTriangles=" << triangleVertexArray->getNbTriangles() << std::endl;
ss << "vertices=[";
// For each triangle of the concave mesh
for (uint v=0; v<triangleVertexArray->getNbVertices(); v++) {
Vector3 vertex;
triangleVertexArray->getVertex(v, &vertex);
ss << vertex.to_string() << ", ";
}
ss << "], " << std::endl;
ss << "normals=[";
// For each triangle of the concave mesh
for (uint v=0; v<triangleVertexArray->getNbVertices(); v++) {
Vector3 normal;
triangleVertexArray->getNormal(v, &normal);
ss << normal.to_string() << ", ";
}
ss << "], " << std::endl;
ss << "triangles=[";
// For each triangle of the concave mesh
// For each triangle of the concave mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
uint indices[3];
triangleVertexArray->getTriangleVerticesIndices(triangleIndex, indices);
ss << "(" << indices[0] << "," << indices[1] << "," << indices[2] << "), ";
}
ss << "], " << std::endl;
ss << "}" << std::endl;
}
return ss.str();
}

138
src/collision/shapes/ConcaveMeshShape.h
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -29,13 +29,15 @@
// Libraries
#include "ConcaveShape.h"
#include "collision/broadphase/DynamicAABBTree.h"
#include "collision/TriangleMesh.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/Profiler.h"
#include "containers/List.h"
namespace reactphysics3d {
// Declarations
class ConcaveMeshShape;
class Profiler;
class TriangleShape;
class TriangleMesh;
// class ConvexTriangleAABBOverlapCallback
class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
@ -61,7 +63,7 @@ class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId);
virtual void notifyOverlappingNode(int nodeId) override;
};
@ -70,26 +72,34 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
private :
std::vector<int32> mHitAABBNodes;
List<int32> mHitAABBNodes;
const DynamicAABBTree& mDynamicAABBTree;
const ConcaveMeshShape& mConcaveMeshShape;
ProxyShape* mProxyShape;
RaycastInfo& mRaycastInfo;
const Ray& mRay;
bool mIsHit;
MemoryAllocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public:
// Constructor
ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray)
: mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) {
ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, MemoryAllocator& allocator)
: mHitAABBNodes(allocator), mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) {
}
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray);
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray) override;
/// Raycast all collision shapes that have been collected
void raycastTriangles();
@ -98,12 +108,21 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
bool getIsHit() const {
return mIsHit;
}
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
};
// Class ConcaveMeshShape
/**
* This class represents a static concave mesh shape. Note that collision detection
* with a concave mesh shape can be very expensive. You should use only use
* with a concave mesh shape can be very expensive. You should only use
* this shape for a static mesh.
*/
class ConcaveMeshShape : public ConcaveShape {
@ -118,47 +137,68 @@ class ConcaveMeshShape : public ConcaveShape {
/// Dynamic AABB tree to accelerate collision with the triangles
DynamicAABBTree mDynamicAABBTree;
/// Array with computed vertices normals for each TriangleVertexArray of the triangle mesh (only
/// if the user did not provide its own vertices normals)
Vector3** mComputedVerticesNormals;
/// Scaling
const Vector3 mScaling;
// -------------------- Methods -------------------- //
/// Private copy-constructor
ConcaveMeshShape(const ConcaveMeshShape& shape);
/// Private assignment operator
ConcaveMeshShape& operator=(const ConcaveMeshShape& shape);
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
virtual size_t getSizeInBytes() const override;
/// Insert all the triangles into the dynamic AABB tree
void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
Vector3* outTriangleVertices) const;
void getTriangleVertices(uint subPart, uint triangleIndex, Vector3* outTriangleVertices) const;
/// Return the three vertex normals (in the array outVerticesNormals) of a triangle
void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const;
/// Compute the shape Id for a given triangle of the mesh
uint computeTriangleShapeId(uint subPart, uint triangleIndex) const;
public:
/// Constructor
ConcaveMeshShape(TriangleMesh* triangleMesh);
ConcaveMeshShape(TriangleMesh* triangleMesh, const Vector3& scaling = Vector3(1, 1, 1));
/// Destructor
~ConcaveMeshShape();
virtual ~ConcaveMeshShape() = default;
/// Deleted copy-constructor
ConcaveMeshShape(const ConcaveMeshShape& shape) = delete;
/// Deleted assignment operator
ConcaveMeshShape& operator=(const ConcaveMeshShape& shape) = delete;
/// Return the scaling vector
const Vector3& getScaling() const;
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const override;
/// Return the string representation of the shape
virtual std::string to_string() const override;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler) override;
#endif
// ---------- Friendship ----------- //
@ -171,6 +211,11 @@ inline size_t ConcaveMeshShape::getSizeInBytes() const {
return sizeof(ConcaveMeshShape);
}
// Return the scaling vector
inline const Vector3& ConcaveMeshShape::getScaling() const {
return mScaling;
}
// Return the local bounds of the shape in x, y and z directions.
// This method is used to compute the AABB of the box
/**
@ -186,18 +231,6 @@ inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
max = treeAABB.getMax();
}
// Set the local scaling vector of the collision shape
inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) {
CollisionShape::setLocalScaling(scaling);
// Reset the Dynamic AABB Tree
mDynamicAABBTree.reset();
// Rebuild Dynamic AABB Tree here
initBVHTree();
}
// Return the local inertia tensor of the shape
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
@ -224,12 +257,29 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints);
// Get the vertices normals of the triangle
Vector3 verticesNormals[3];
mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Call the callback to test narrow-phase collision with this triangle
mTriangleTestCallback.testTriangle(trianglePoints);
mTriangleTestCallback.testTriangle(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]));
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void ConcaveMeshShape::setProfiler(Profiler* profiler) {
CollisionShape::setProfiler(profiler);
mDynamicAABBTree.setProfiler(profiler);
}
#endif
}
#endif

12
src/collision/shapes/ConcaveShape.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -31,13 +31,7 @@
using namespace reactphysics3d;
// Constructor
ConcaveShape::ConcaveShape(CollisionShapeType type)
: CollisionShape(type), mIsSmoothMeshCollisionEnabled(false),
mTriangleMargin(0), mRaycastTestType(FRONT) {
}
// Destructor
ConcaveShape::~ConcaveShape() {
ConcaveShape::ConcaveShape(CollisionShapeName name)
: CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mRaycastTestType(TriangleRaycastSide::FRONT) {
}

Some files were not shown because too many files have changed in this diff Show More