atria-soft/ephysics
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[DEV] continue rework

Browse Source
This commit is contained in:
Edouard DUPIN 2017-06-08 22:35:22 +02:00
parent 562af6c0ae
commit 0f6adcf289
224 changed files with 28906 additions and 31578 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
ephysics/body/Body.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/body/Body.h> #include <ephysics/body/Body.h>
@ -35,8 +16,8 @@ using namespace reactphysics3d;
* @param id ID of the new body * @param id ID of the new body
*/ */
Body::Body(bodyindex id) Body::Body(bodyindex id)
: mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true), : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true),
mIsSleeping(false), mSleepTime(0), mUserData(NULL) { mIsSleeping(false), mSleepTime(0), mUserData(NULL) {
} }

207
ephysics/body/Body.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_BODY_H
#define REACTPHYSICS3D_BODY_H
// Libraries // Libraries
#include <stdexcept> #include <stdexcept>
@ -43,98 +22,98 @@ namespace reactphysics3d {
*/ */
class Body { class Body {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// ID of the body /// ID of the body
bodyindex mID; bodyindex mID;
/// True if the body has already been added in an island (for sleeping technique) /// True if the body has already been added in an island (for sleeping technique)
bool mIsAlreadyInIsland; bool mIsAlreadyInIsland;
/// True if the body is allowed to go to sleep for better efficiency /// True if the body is allowed to go to sleep for better efficiency
bool mIsAllowedToSleep; bool mIsAllowedToSleep;
/// True if the body is active. /// True if the body is active.
/// An inactive body does not participate in collision detection, /// An inactive body does not participate in collision detection,
/// is not simulated and will not be hit in a ray casting query. /// is not simulated and will not be hit in a ray casting query.
/// A body is active by default. If you set this /// A body is active by default. If you set this
/// value to "false", all the proxy shapes of this body will be /// value to "false", all the proxy shapes of this body will be
/// removed from the broad-phase. If you set this value to "true", /// removed from the broad-phase. If you set this value to "true",
/// all the proxy shapes will be added to the broad-phase. A joint /// all the proxy shapes will be added to the broad-phase. A joint
/// connected to an inactive body will also be inactive. /// connected to an inactive body will also be inactive.
bool mIsActive; bool mIsActive;
/// True if the body is sleeping (for sleeping technique) /// True if the body is sleeping (for sleeping technique)
bool mIsSleeping; bool mIsSleeping;
/// Elapsed time since the body velocity was bellow the sleep velocity /// Elapsed time since the body velocity was bellow the sleep velocity
decimal mSleepTime; float mSleepTime;
/// Pointer that can be used to attach user data to the body /// Pointer that can be used to attach user data to the body
void* mUserData; void* mUserData;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
Body(const Body& body); Body(const Body& body);
/// Private assignment operator /// Private assignment operator
Body& operator=(const Body& body); Body& operator=(const Body& body);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Body(bodyindex id); Body(bodyindex id);
/// Destructor /// Destructor
virtual ~Body(); virtual ~Body();
/// Return the ID of the body /// Return the ID of the body
bodyindex getID() const; bodyindex getID() const;
/// Return whether or not the body is allowed to sleep /// Return whether or not the body is allowed to sleep
bool isAllowedToSleep() const; bool isAllowedToSleep() const;
/// Set whether or not the body is allowed to go to sleep /// Set whether or not the body is allowed to go to sleep
void setIsAllowedToSleep(bool isAllowedToSleep); void setIsAllowedToSleep(bool isAllowedToSleep);
/// Set the variable to know whether or not the body is sleeping /// Set the variable to know whether or not the body is sleeping
virtual void setIsSleeping(bool isSleeping); virtual void setIsSleeping(bool isSleeping);
/// Return whether or not the body is sleeping /// Return whether or not the body is sleeping
bool isSleeping() const; bool isSleeping() const;
/// Return true if the body is active /// Return true if the body is active
bool isActive() const; bool isActive() const;
/// Set whether or not the body is active /// Set whether or not the body is active
virtual void setIsActive(bool isActive); virtual void setIsActive(bool isActive);
/// Return a pointer to the user data attached to this body /// Return a pointer to the user data attached to this body
void* getUserData() const; void* getUserData() const;
/// Attach user data to this body /// Attach user data to this body
void setUserData(void* userData); void setUserData(void* userData);
/// Smaller than operator /// Smaller than operator
bool operator<(const Body& body2) const; bool operator<(const Body& body2) const;
/// Larger than operator /// Larger than operator
bool operator>(const Body& body2) const; bool operator>(const Body& body2) const;
/// Equal operator /// Equal operator
bool operator==(const Body& body2) const; bool operator==(const Body& body2) const;
/// Not equal operator /// Not equal operator
bool operator!=(const Body& body2) const; bool operator!=(const Body& body2) const;
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;
}; };
// Return the id of the body // Return the id of the body
@ -142,7 +121,7 @@ class 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; return mID;
} }
// Return whether or not the body is allowed to sleep // Return whether or not the body is allowed to sleep
@ -150,7 +129,7 @@ inline bodyindex Body::getID() const {
* @return True if the body is allowed to sleep and false otherwise * @return True if the body is allowed to sleep and false otherwise
*/ */
inline bool Body::isAllowedToSleep() const { inline bool Body::isAllowedToSleep() const {
return mIsAllowedToSleep; return mIsAllowedToSleep;
} }
// Set whether or not the body is allowed to go to sleep // Set whether or not the body is allowed to go to sleep
@ -158,9 +137,9 @@ inline bool Body::isAllowedToSleep() const {
* @param isAllowedToSleep True if the body is allowed to sleep * @param isAllowedToSleep True if the body is allowed to sleep
*/ */
inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) { inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
mIsAllowedToSleep = isAllowedToSleep; mIsAllowedToSleep = isAllowedToSleep;
if (!mIsAllowedToSleep) setIsSleeping(false); if (!mIsAllowedToSleep) setIsSleeping(false);
} }
// Return whether or not the body is sleeping // Return whether or not the body is sleeping
@ -168,7 +147,7 @@ inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
* @return True if the body is currently sleeping and false otherwise * @return True if the body is currently sleeping and false otherwise
*/ */
inline bool Body::isSleeping() const { inline bool Body::isSleeping() const {
return mIsSleeping; return mIsSleeping;
} }
// Return true if the body is active // Return true if the body is active
@ -176,7 +155,7 @@ inline bool Body::isSleeping() const {
* @return True if the body currently active and false otherwise * @return True if the body currently active and false otherwise
*/ */
inline bool Body::isActive() const { inline bool Body::isActive() const {
return mIsActive; return mIsActive;
} }
// Set whether or not the body is active // Set whether or not the body is active
@ -184,22 +163,22 @@ inline bool Body::isActive() const {
* @param isActive True if you want to activate the body * @param isActive True if you want to activate the body
*/ */
inline void Body::setIsActive(bool isActive) { inline void Body::setIsActive(bool isActive) {
mIsActive = isActive; mIsActive = isActive;
} }
// Set the variable to know whether or not the body is sleeping // Set the variable to know whether or not the body is sleeping
inline void Body::setIsSleeping(bool isSleeping) { inline void Body::setIsSleeping(bool isSleeping) {
if (isSleeping) { if (isSleeping) {
mSleepTime = decimal(0.0); mSleepTime = float(0.0);
} }
else { else {
if (mIsSleeping) { if (mIsSleeping) {
mSleepTime = decimal(0.0); mSleepTime = float(0.0);
} }
} }
mIsSleeping = isSleeping; mIsSleeping = isSleeping;
} }
// Return a pointer to the user data attached to this body // Return a pointer to the user data attached to this body
@ -207,7 +186,7 @@ inline void Body::setIsSleeping(bool isSleeping) {
* @return A pointer to the user data you have attached to the body * @return A pointer to the user data you have attached to the body
*/ */
inline void* Body::getUserData() const { inline void* Body::getUserData() const {
return mUserData; return mUserData;
} }
// Attach user data to this body // Attach user data to this body
@ -215,29 +194,27 @@ inline void* Body::getUserData() const {
* @param userData A pointer to the user data you want to attach to the body * @param userData A pointer to the user data you want to attach to the body
*/ */
inline void Body::setUserData(void* userData) { inline void Body::setUserData(void* userData) {
mUserData = userData; mUserData = userData;
} }
// Smaller than operator // Smaller than operator
inline bool Body::operator<(const Body& body2) const { inline bool Body::operator<(const Body& body2) const {
return (mID < body2.mID); return (mID < body2.mID);
} }
// Larger than operator // Larger than operator
inline bool Body::operator>(const Body& body2) const { inline bool Body::operator>(const Body& body2) const {
return (mID > body2.mID); return (mID > body2.mID);
} }
// Equal operator // Equal operator
inline bool Body::operator==(const Body& body2) const { inline bool Body::operator==(const Body& body2) const {
return (mID == body2.mID); return (mID == body2.mID);
} }
// Not equal operator // Not equal operator
inline bool Body::operator!=(const Body& body2) const { inline bool Body::operator!=(const Body& body2) const {
return (mID != body2.mID); return (mID != body2.mID);
} }
} }
#endif

355
ephysics/body/CollisionBody.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/body/CollisionBody.h> #include <ephysics/body/CollisionBody.h>
@ -38,17 +19,17 @@ using namespace reactphysics3d;
* @param id ID of the body * @param id ID of the body
*/ */
CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id) CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL), : Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL),
mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) { mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) {
} }
// Destructor // Destructor
CollisionBody::~CollisionBody() { CollisionBody::~CollisionBody() {
assert(mContactManifoldsList == NULL); assert(mContactManifoldsList == NULL);
// Remove all the proxy collision shapes of the body // Remove all the proxy collision shapes of the body
removeAllCollisionShapes(); removeAllCollisionShapes();
} }
// Add a collision shape to the body. Note that you can share a collision // Add a collision shape to the body. Note that you can share a collision
@ -62,38 +43,38 @@ CollisionBody::~CollisionBody() {
/** /**
* @param collisionShape A pointer to the collision shape you want to add to the body * @param collisionShape A pointer to the collision shape you want to add to the body
* @param transform The transformation of the collision shape that transforms the * @param transform The transformation of the collision shape that transforms the
* local-space of the collision shape into the local-space of the body * local-space of the collision shape int32_to the local-space of the body
* @return A pointer to the proxy shape that has been created to link the body to * @return A pointer to the proxy shape that has been created to link the body to
* the new collision shape you have added. * the new collision shape you have added.
*/ */
ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform) { const Transform& transform) {
// Create a new proxy collision shape to attach the collision shape to the body // Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, collisionShape, sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, decimal(1)); transform, float(1));
// Add it to the list of proxy collision shapes of the body // Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == NULL) { if (mProxyCollisionShapes == NULL) {
mProxyCollisionShapes = proxyShape; mProxyCollisionShapes = proxyShape;
} }
else { else {
proxyShape->mNext = mProxyCollisionShapes; proxyShape->mNext = mProxyCollisionShapes;
mProxyCollisionShapes = proxyShape; mProxyCollisionShapes = proxyShape;
} }
// Compute the world-space AABB of the new collision shape // Compute the world-space AABB of the new collision shape
AABB aabb; AABB aabb;
collisionShape->computeAABB(aabb, mTransform * transform); collisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape // Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
mNbCollisionShapes++; mNbCollisionShapes++;
// Return a pointer to the collision shape // Return a pointer to the collision shape
return proxyShape; return proxyShape;
} }
// Remove a collision shape from the body // Remove a collision shape from the body
@ -105,109 +86,109 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
*/ */
void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* current = mProxyCollisionShapes; ProxyShape* current = mProxyCollisionShapes;
// If the the first proxy shape is the one to remove // If the the first proxy shape is the one to remove
if (current == proxyShape) { if (current == proxyShape) {
mProxyCollisionShapes = current->mNext; mProxyCollisionShapes = current->mNext;
if (mIsActive) { if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current); mWorld.mCollisionDetection.removeProxyCollisionShape(current);
} }
current->~ProxyShape(); current->~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
mNbCollisionShapes--; mNbCollisionShapes--;
return; return;
} }
// Look for the proxy shape that contains the collision shape in parameter // Look for the proxy shape that contains the collision shape in parameter
while(current->mNext != NULL) { while(current->mNext != NULL) {
// If we have found the collision shape to remove // If we have found the collision shape to remove
if (current->mNext == proxyShape) { if (current->mNext == proxyShape) {
// Remove the proxy collision shape // Remove the proxy collision shape
ProxyShape* elementToRemove = current->mNext; ProxyShape* elementToRemove = current->mNext;
current->mNext = elementToRemove->mNext; current->mNext = elementToRemove->mNext;
if (mIsActive) { if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove); mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
} }
elementToRemove->~ProxyShape(); elementToRemove->~ProxyShape();
mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape)); mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape));
mNbCollisionShapes--; mNbCollisionShapes--;
return; return;
} }
// Get the next element in the list // Get the next element in the list
current = current->mNext; current = current->mNext;
} }
} }
// Remove all the collision shapes // Remove all the collision shapes
void CollisionBody::removeAllCollisionShapes() { void CollisionBody::removeAllCollisionShapes() {
ProxyShape* current = mProxyCollisionShapes; ProxyShape* current = mProxyCollisionShapes;
// Look for the proxy shape that contains the collision shape in parameter // Look for the proxy shape that contains the collision shape in parameter
while(current != NULL) { while(current != NULL) {
// Remove the proxy collision shape // Remove the proxy collision shape
ProxyShape* nextElement = current->mNext; ProxyShape* nextElement = current->mNext;
if (mIsActive) { if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current); mWorld.mCollisionDetection.removeProxyCollisionShape(current);
} }
current->~ProxyShape(); current->~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
// Get the next element in the list // Get the next element in the list
current = nextElement; current = nextElement;
} }
mProxyCollisionShapes = NULL; mProxyCollisionShapes = NULL;
} }
// Reset the contact manifold lists // Reset the contact manifold lists
void CollisionBody::resetContactManifoldsList() { void CollisionBody::resetContactManifoldsList() {
// Delete the linked list of contact manifolds of that body // Delete the linked list of contact manifolds of that body
ContactManifoldListElement* currentElement = mContactManifoldsList; ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != NULL) { while (currentElement != NULL) {
ContactManifoldListElement* nextElement = currentElement->next; ContactManifoldListElement* nextElement = currentElement->next;
// Delete the current element // Delete the current element
currentElement->~ContactManifoldListElement(); currentElement->~ContactManifoldListElement();
mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
currentElement = nextElement; currentElement = nextElement;
} }
mContactManifoldsList = NULL; mContactManifoldsList = NULL;
} }
// Update the broad-phase state for this body (because it has moved for instance) // Update the broad-phase state for this body (because it has moved for instance)
void CollisionBody::updateBroadPhaseState() const { void CollisionBody::updateBroadPhaseState() const {
// For all the proxy collision shapes of the body // For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Update the proxy // Update the proxy
updateProxyShapeInBroadPhase(shape); updateProxyShapeInBroadPhase(shape);
} }
} }
// Update the broad-phase state of a proxy collision shape of the body // Update the broad-phase state of a proxy collision shape of the body
void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const { void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const {
// Recompute the world-space AABB of the collision shape // Recompute the world-space AABB of the collision shape
AABB aabb; AABB aabb;
proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape // Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert); mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert);
} }
// Set whether or not the body is active // Set whether or not the body is active
@ -216,68 +197,68 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo
*/ */
void CollisionBody::setIsActive(bool isActive) { void CollisionBody::setIsActive(bool isActive) {
// If the state does not change // If the state does not change
if (mIsActive == isActive) return; if (mIsActive == isActive) return;
Body::setIsActive(isActive); Body::setIsActive(isActive);
// If we have to activate the body // If we have to activate the body
if (isActive) { if (isActive) {
// For each proxy shape of the body // For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Compute the world-space AABB of the new collision shape // Compute the world-space AABB of the new collision shape
AABB aabb; AABB aabb;
shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform); shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform);
// Add the proxy shape to the collision detection // Add the proxy shape to the collision detection
mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb); mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb);
} }
} }
else { // If we have to deactivate the body else { // If we have to deactivate the body
// For each proxy shape of the body // For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Remove the proxy shape from the collision detection // Remove the proxy shape from the collision detection
mWorld.mCollisionDetection.removeProxyCollisionShape(shape); mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
} }
// Reset the contact manifold list of the body // Reset the contact manifold list of the body
resetContactManifoldsList(); resetContactManifoldsList();
} }
} }
// Ask the broad-phase to test again the collision shapes of the body for collision // Ask the broad-phase to test again the collision shapes of the body for collision
// (as if the body has moved). // (as if the body has moved).
void CollisionBody::askForBroadPhaseCollisionCheck() const { void CollisionBody::askForBroadPhaseCollisionCheck() const {
// For all the proxy collision shapes of the body // For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape); mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape);
} }
} }
// Reset the mIsAlreadyInIsland variable of the body and contact manifolds. // Reset the mIsAlreadyInIsland variable of the body and contact manifolds.
/// This method also returns the number of contact manifolds of the body. /// This method also returns the number of contact manifolds of the body.
int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
mIsAlreadyInIsland = false; mIsAlreadyInIsland = false;
int nbManifolds = 0; int32_t nbManifolds = 0;
// Reset the mIsAlreadyInIsland variable of the contact manifolds for // Reset the mIsAlreadyInIsland variable of the contact manifolds for
// this body // this body
ContactManifoldListElement* currentElement = mContactManifoldsList; ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != NULL) { while (currentElement != NULL) {
currentElement->contactManifold->mIsAlreadyInIsland = false; currentElement->contactManifold->mIsAlreadyInIsland = false;
currentElement = currentElement->next; currentElement = currentElement->next;
nbManifolds++; nbManifolds++;
} }
return nbManifolds; return nbManifolds;
} }
// Return true if a point is inside the collision body // Return true if a point is inside the collision body
@ -288,14 +269,14 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
*/ */
bool CollisionBody::testPointInside(const Vector3& worldPoint) const { bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
// For each collision shape of the body // For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Test if the point is inside the collision shape // Test if the point is inside the collision shape
if (shape->testPointInside(worldPoint)) return true; if (shape->testPointInside(worldPoint)) return true;
} }
return false; return false;
} }
// Raycast method with feedback information // Raycast method with feedback information
@ -303,28 +284,28 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
/** /**
* @param ray The ray used to raycast agains the body * @param ray The ray used to raycast agains the body
* @param[out] raycastInfo Structure that contains the result of the raycasting * @param[out] raycastInfo Structure that contains the result of the raycasting
* (valid only if the method returned true) * (valid only if the method returned true)
* @return True if the ray hit the body and false otherwise * @return True if the ray hit the body and false otherwise
*/ */
bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) { bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the body is not active, it cannot be hit by rays // If the body is not active, it cannot be hit by rays
if (!mIsActive) return false; if (!mIsActive) return false;
bool isHit = false; bool isHit = false;
Ray rayTemp(ray); Ray rayTemp(ray);
// For each collision shape of the body // For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Test if the ray hits the collision shape // Test if the ray hits the collision shape
if (shape->raycast(rayTemp, raycastInfo)) { if (shape->raycast(rayTemp, raycastInfo)) {
rayTemp.maxFraction = raycastInfo.hitFraction; rayTemp.maxFraction = raycastInfo.hitFraction;
isHit = true; isHit = true;
} }
} }
return isHit; return isHit;
} }
// Compute and return the AABB of the body by merging all proxy shapes AABBs // Compute and return the AABB of the body by merging all proxy shapes AABBs
@ -333,22 +314,22 @@ bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
*/ */
AABB CollisionBody::getAABB() const { AABB CollisionBody::getAABB() const {
AABB bodyAABB; AABB bodyAABB;
if (mProxyCollisionShapes == NULL) return bodyAABB; if (mProxyCollisionShapes == NULL) return bodyAABB;
mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform()); mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform());
// For each proxy shape of the body // For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) {
// Compute the world-space AABB of the collision shape // Compute the world-space AABB of the collision shape
AABB aabb; AABB aabb;
shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform()); shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform());
// Merge the proxy shape AABB with the current body AABB // Merge the proxy shape AABB with the current body AABB
bodyAABB.mergeWithAABB(aabb); bodyAABB.mergeWithAABB(aabb);
} }
return bodyAABB; return bodyAABB;
} }

261
ephysics/body/CollisionBody.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_BODY_H
#define REACTPHYSICS3D_COLLISION_BODY_H
// Libraries // Libraries
#include <stdexcept> #include <stdexcept>
@ -47,13 +26,13 @@ class CollisionWorld;
/// Enumeration for the type of a body /// Enumeration for the type of a body
/// STATIC : A static body has infinite mass, zero velocity but the position can be /// 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. /// 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 /// 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 /// position is computed by the physics engine. A kinematic body does not collide with
/// other static or kinematic bodies. /// other static or kinematic bodies.
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its /// 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 /// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies. /// dynamic, static or kinematic bodies.
enum BodyType {STATIC, KINEMATIC, DYNAMIC}; enum BodyType {STATIC, KINEMATIC, DYNAMIC};
// Class CollisionBody // Class CollisionBody
@ -63,125 +42,125 @@ enum BodyType {STATIC, KINEMATIC, DYNAMIC};
*/ */
class CollisionBody : public Body { class CollisionBody : public Body {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Type of body (static, kinematic or dynamic) /// Type of body (static, kinematic or dynamic)
BodyType mType; BodyType mType;
/// Position and orientation of the body /// Position and orientation of the body
Transform mTransform; Transform mTransform;
/// First element of the linked list of proxy collision shapes of this body /// First element of the linked list of proxy collision shapes of this body
ProxyShape* mProxyCollisionShapes; ProxyShape* mProxyCollisionShapes;
/// Number of collision shapes /// Number of collision shapes
uint mNbCollisionShapes; uint32_t mNbCollisionShapes;
/// First element of the linked list of contact manifolds involving this body /// First element of the linked list of contact manifolds involving this body
ContactManifoldListElement* mContactManifoldsList; ContactManifoldListElement* mContactManifoldsList;
/// Reference to the world the body belongs to /// Reference to the world the body belongs to
CollisionWorld& mWorld; CollisionWorld& mWorld;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
CollisionBody(const CollisionBody& body); CollisionBody(const CollisionBody& body);
/// Private assignment operator /// Private assignment operator
CollisionBody& operator=(const CollisionBody& body); CollisionBody& operator=(const CollisionBody& body);
/// Reset the contact manifold lists /// Reset the contact manifold lists
void resetContactManifoldsList(); void resetContactManifoldsList();
/// Remove all the collision shapes /// Remove all the collision shapes
void removeAllCollisionShapes(); void removeAllCollisionShapes();
/// Update the broad-phase state for this body (because it has moved for instance) /// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const; virtual void updateBroadPhaseState() const;
/// Update the broad-phase state of a proxy collision shape of the body /// Update the broad-phase state of a proxy collision shape of the body
void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert = false) const; void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert = false) const;
/// Ask the broad-phase to test again the collision shapes of the body for collision /// Ask the broad-phase to test again the collision shapes of the body for collision
/// (as if the body has moved). /// (as if the body has moved).
void askForBroadPhaseCollisionCheck() const; void askForBroadPhaseCollisionCheck() const;
/// Reset the mIsAlreadyInIsland variable of the body and contact manifolds /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
int resetIsAlreadyInIslandAndCountManifolds(); int32_t resetIsAlreadyInIslandAndCountManifolds();
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id); CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id);
/// Destructor /// Destructor
virtual ~CollisionBody(); virtual ~CollisionBody();
/// Return the type of the body /// Return the type of the body
BodyType getType() const; BodyType getType() const;
/// Set the type of the body /// Set the type of the body
void setType(BodyType type); void setType(BodyType type);
/// Set whether or not the body is active /// Set whether or not the body is active
virtual void setIsActive(bool isActive); virtual void setIsActive(bool isActive);
/// Return the current position and orientation /// Return the current position and orientation
const Transform& getTransform() const; const Transform& getTransform() const;
/// Set the current position and orientation /// Set the current position and orientation
virtual void setTransform(const Transform& transform); virtual void setTransform(const Transform& transform);
/// Add a collision shape to the body. /// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
const Transform& transform); const Transform& transform);
/// Remove a collision shape from the body /// Remove a collision shape from the body
virtual void removeCollisionShape(const ProxyShape* proxyShape); virtual void removeCollisionShape(const ProxyShape* proxyShape);
/// Return the first element of the linked list of contact manifolds involving this body /// Return the first element of the linked list of contact manifolds involving this body
const ContactManifoldListElement* getContactManifoldsList() const; const ContactManifoldListElement* getContactManifoldsList() const;
/// Return true if a point is inside the collision body /// Return true if a point is inside the collision body
bool testPointInside(const Vector3& worldPoint) const; bool testPointInside(const Vector3& worldPoint) const;
/// Raycast method with feedback information /// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo); bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
/// Compute and return the AABB of the body by merging all proxy shapes AABBs /// Compute and return the AABB of the body by merging all proxy shapes AABBs
AABB getAABB() const; AABB getAABB() const;
/// Return the linked list of proxy shapes of that body /// Return the linked list of proxy shapes of that body
ProxyShape* getProxyShapesList(); ProxyShape* getProxyShapesList();
/// Return the linked list of proxy shapes of that body /// Return the linked list of proxy shapes of that body
const ProxyShape* getProxyShapesList() const; const ProxyShape* getProxyShapesList() const;
/// Return the world-space coordinates of a point given the local-space coordinates of the body /// Return the world-space coordinates of a point given the local-space coordinates of the body
Vector3 getWorldPoint(const Vector3& localPoint) const; Vector3 getWorldPoint(const Vector3& localPoint) const;
/// Return the world-space vector of a vector given in local-space coordinates of the body /// Return the world-space vector of a vector given in local-space coordinates of the body
Vector3 getWorldVector(const Vector3& localVector) const; Vector3 getWorldVector(const Vector3& localVector) const;
/// Return the body local-space coordinates of a point given in the world-space coordinates /// Return the body local-space coordinates of a point given in the world-space coordinates
Vector3 getLocalPoint(const Vector3& worldPoint) const; Vector3 getLocalPoint(const Vector3& worldPoint) const;
/// Return the body local-space coordinates of a vector given in the world-space coordinates /// Return the body local-space coordinates of a vector given in the world-space coordinates
Vector3 getLocalVector(const Vector3& worldVector) const; Vector3 getLocalVector(const Vector3& worldVector) const;
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class CollisionWorld; friend class CollisionWorld;
friend class DynamicsWorld; friend class DynamicsWorld;
friend class CollisionDetection; friend class CollisionDetection;
friend class BroadPhaseAlgorithm; friend class BroadPhaseAlgorithm;
friend class ConvexMeshShape; friend class ConvexMeshShape;
friend class ProxyShape; friend class ProxyShape;
}; };
// Return the type of the body // Return the type of the body
@ -189,80 +168,80 @@ class CollisionBody : public Body {
* @return the type of the body (STATIC, KINEMATIC, DYNAMIC) * @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
*/ */
inline BodyType CollisionBody::getType() const { inline BodyType CollisionBody::getType() const {
return mType; return mType;
} }
// Set the type of the body // Set the type of the body
/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow: /// 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 /// 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. /// 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 /// 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 /// position is computed by the physics engine. A kinematic body does not collide with
/// other static or kinematic bodies. /// other static or kinematic bodies.
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its /// 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 /// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies. /// dynamic, static or kinematic bodies.
/** /**
* @param type The type of the body (STATIC, KINEMATIC, DYNAMIC) * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
*/ */
inline void CollisionBody::setType(BodyType type) { inline void CollisionBody::setType(BodyType type) {
mType = type; mType = type;
if (mType == STATIC) { if (mType == STATIC) {
// Update the broad-phase state of the body // Update the broad-phase state of the body
updateBroadPhaseState(); updateBroadPhaseState();
} }
} }
// Return the current position and orientation // Return the current position and orientation
/** /**
* @return The current transformation of the body that transforms the local-space * @return The current transformation of the body that transforms the local-space
* of the body into world-space * of the body int32_to world-space
*/ */
inline const Transform& CollisionBody::getTransform() const { inline const Transform& CollisionBody::getTransform() const {
return mTransform; return mTransform;
} }
// Set the current position and orientation // Set the current position and orientation
/** /**
* @param transform The transformation of the body that transforms the local-space * @param transform The transformation of the body that transforms the local-space
* of the body into world-space * of the body int32_to world-space
*/ */
inline void CollisionBody::setTransform(const Transform& transform) { inline void CollisionBody::setTransform(const Transform& transform) {
// Update the transform of the body // Update the transform of the body
mTransform = transform; mTransform = transform;
// Update the broad-phase state of the body // Update the broad-phase state of the body
updateBroadPhaseState(); updateBroadPhaseState();
} }
// Return the first element of the linked list of contact manifolds involving this body // 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 * @return A pointer to the first element of the linked-list with the contact
* manifolds of this body * manifolds of this body
*/ */
inline const ContactManifoldListElement* CollisionBody::getContactManifoldsList() const { inline const ContactManifoldListElement* CollisionBody::getContactManifoldsList() const {
return mContactManifoldsList; return mContactManifoldsList;
} }
// Return the linked list of proxy shapes of that body // Return the linked list of proxy shapes of that body
/** /**
* @return The pointer of the first proxy shape of the linked-list of all the * @return The pointer of the first proxy shape of the linked-list of all the
* proxy shapes of the body * proxy shapes of the body
*/ */
inline ProxyShape* CollisionBody::getProxyShapesList() { inline ProxyShape* CollisionBody::getProxyShapesList() {
return mProxyCollisionShapes; return mProxyCollisionShapes;
} }
// Return the linked list of proxy shapes of that body // Return the linked list of proxy shapes of that body
/** /**
* @return The pointer of the first proxy shape of the linked-list of all the * @return The pointer of the first proxy shape of the linked-list of all the
* proxy shapes of the body * proxy shapes of the body
*/ */
inline const ProxyShape* CollisionBody::getProxyShapesList() const { inline const ProxyShape* CollisionBody::getProxyShapesList() const {
return mProxyCollisionShapes; return mProxyCollisionShapes;
} }
// Return the world-space coordinates of a point given the local-space coordinates of the body // Return the world-space coordinates of a point given the local-space coordinates of the body
@ -271,7 +250,7 @@ inline const ProxyShape* CollisionBody::getProxyShapesList() const {
* @return The point in world-space coordinates * @return The point in world-space coordinates
*/ */
inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const { inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const {
return mTransform * localPoint; return mTransform * localPoint;
} }
// Return the world-space vector of a vector given in local-space coordinates of the body // Return the world-space vector of a vector given in local-space coordinates of the body
@ -280,7 +259,7 @@ inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const {
* @return The vector in world-space coordinates * @return The vector in world-space coordinates
*/ */
inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const { inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const {
return mTransform.getOrientation() * localVector; return mTransform.getOrientation() * localVector;
} }
// Return the body local-space coordinates of a point given in the world-space coordinates // Return the body local-space coordinates of a point given in the world-space coordinates
@ -289,7 +268,7 @@ inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const {
* @return The point in the local-space coordinates of the body * @return The point in the local-space coordinates of the body
*/ */
inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const { inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const {
return mTransform.getInverse() * worldPoint; return mTransform.getInverse() * worldPoint;
} }
// Return the body local-space coordinates of a vector given in the world-space coordinates // Return the body local-space coordinates of a vector given in the world-space coordinates
@ -298,9 +277,7 @@ inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const {
* @return The vector in the local-space coordinates of the body * @return The vector in the local-space coordinates of the body
*/ */
inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const { inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
return mTransform.getOrientation().getInverse() * worldVector; return mTransform.getOrientation().getInverse() * worldVector;
} }
} }
#endif

453
ephysics/body/RigidBody.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/body/RigidBody.h> #include <ephysics/body/RigidBody.h>
@ -39,163 +20,163 @@ using namespace reactphysics3d;
* @param id The ID of the body * @param id The ID of the body
*/ */
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id) RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: CollisionBody(transform, world, id), mInitMass(decimal(1.0)), : CollisionBody(transform, world, id), mInitMass(float(1.0)),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mIsGravityEnabled(true), mLinearDamping(float(0.0)), mAngularDamping(float(0.0)),
mJointsList(NULL) { mJointsList(NULL) {
// Compute the inverse mass // Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = float(1.0) / mInitMass;
} }
// Destructor // Destructor
RigidBody::~RigidBody() { RigidBody::~RigidBody() {
assert(mJointsList == NULL); assert(mJointsList == NULL);
} }
// Set the type of the body // Set the type of the body
/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow: /// 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 /// 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. /// 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 /// 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 /// position is computed by the physics engine. A kinematic body does not collide with
/// other static or kinematic bodies. /// other static or kinematic bodies.
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its /// 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 /// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies. /// dynamic, static or kinematic bodies.
/** /**
* @param type The type of the body (STATIC, KINEMATIC, DYNAMIC) * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
*/ */
void RigidBody::setType(BodyType type) { void RigidBody::setType(BodyType type) {
if (mType == type) return; if (mType == type) return;
CollisionBody::setType(type); CollisionBody::setType(type);
// Recompute the total mass, center of mass and inertia tensor // Recompute the total mass, center of mass and inertia tensor
recomputeMassInformation(); recomputeMassInformation();
// If it is a static body // If it is a static body
if (mType == STATIC) { if (mType == STATIC) {
// Reset the velocity to zero // Reset the velocity to zero
mLinearVelocity.setToZero(); mLinearVelocity.setToZero();
mAngularVelocity.setToZero(); mAngularVelocity.setToZero();
} }
// If it is a static or a kinematic body // If it is a static or a kinematic body
if (mType == STATIC || mType == KINEMATIC) { if (mType == STATIC || mType == KINEMATIC) {
// Reset the inverse mass and inverse inertia tensor to zero // Reset the inverse mass and inverse inertia tensor to zero
mMassInverse = decimal(0.0); mMassInverse = float(0.0);
mInertiaTensorLocal.setToZero(); mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero(); mInertiaTensorLocalInverse.setToZero();
} }
else { // If it is a dynamic body else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = float(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
} }
// Awake the body // Awake the body
setIsSleeping(false); setIsSleeping(false);
// Remove all the contacts with this body // Remove all the contacts with this body
resetContactManifoldsList(); resetContactManifoldsList();
// Ask the broad-phase to test again the collision shapes of the body for collision // Ask the broad-phase to test again the collision shapes of the body for collision
// detection (as if the body has moved) // detection (as if the body has moved)
askForBroadPhaseCollisionCheck(); askForBroadPhaseCollisionCheck();
// Reset the force and torque on the body // Reset the force and torque on the body
mExternalForce.setToZero(); mExternalForce.setToZero();
mExternalTorque.setToZero(); mExternalTorque.setToZero();
} }
// Set the local inertia tensor of the body (in local-space coordinates) // Set the local inertia tensor of the body (in local-space coordinates)
/** /**
* @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space * @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space
* coordinates * coordinates
*/ */
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
mInertiaTensorLocal = inertiaTensorLocal; mInertiaTensorLocal = inertiaTensorLocal;
// Compute the inverse local inertia tensor // Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
} }
// Set the local center of mass of the body (in local-space coordinates) // Set the local center of mass of the body (in local-space coordinates)
/** /**
* @param centerOfMassLocal The center of mass of the body in local-space * @param centerOfMassLocal The center of mass of the body in local-space
* coordinates * coordinates
*/ */
void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
const Vector3 oldCenterOfMass = mCenterOfMassWorld; const Vector3 oldCenterOfMass = mCenterOfMassWorld;
mCenterOfMassLocal = centerOfMassLocal; mCenterOfMassLocal = centerOfMassLocal;
// Compute the center of mass in world-space coordinates // Compute the center of mass in world-space coordinates
mCenterOfMassWorld = mTransform * mCenterOfMassLocal; mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
// Update the linear velocity of the center of mass // Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
} }
// Set the mass of the rigid body // Set the mass of the rigid body
/** /**
* @param mass The mass (in kilograms) of the body * @param mass The mass (in kilograms) of the body
*/ */
void RigidBody::setMass(decimal mass) { void RigidBody::setMass(float mass) {
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
mInitMass = mass; mInitMass = mass;
if (mInitMass > decimal(0.0)) { if (mInitMass > float(0.0)) {
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = float(1.0) / mInitMass;
} }
else { else {
mInitMass = decimal(1.0); mInitMass = float(1.0);
mMassInverse = decimal(1.0); mMassInverse = float(1.0);
} }
} }
// Remove a joint from the joints list // Remove a joint from the joints list
void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) { void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) {
assert(joint != NULL); assert(joint != NULL);
assert(mJointsList != NULL); assert(mJointsList != NULL);
// Remove the joint from the linked list of the joints of the first body // 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 if (mJointsList->joint == joint) { // If the first element is the one to remove
JointListElement* elementToRemove = mJointsList; JointListElement* elementToRemove = mJointsList;
mJointsList = elementToRemove->next; mJointsList = elementToRemove->next;
elementToRemove->~JointListElement(); elementToRemove->~JointListElement();
memoryAllocator.release(elementToRemove, sizeof(JointListElement)); memoryAllocator.release(elementToRemove, sizeof(JointListElement));
} }
else { // If the element to remove is not the first one in the list else { // If the element to remove is not the first one in the list
JointListElement* currentElement = mJointsList; JointListElement* currentElement = mJointsList;
while (currentElement->next != NULL) { while (currentElement->next != NULL) {
if (currentElement->next->joint == joint) { if (currentElement->next->joint == joint) {
JointListElement* elementToRemove = currentElement->next; JointListElement* elementToRemove = currentElement->next;
currentElement->next = elementToRemove->next; currentElement->next = elementToRemove->next;
elementToRemove->~JointListElement(); elementToRemove->~JointListElement();
memoryAllocator.release(elementToRemove, sizeof(JointListElement)); memoryAllocator.release(elementToRemove, sizeof(JointListElement));
break; break;
} }
currentElement = currentElement->next; currentElement = currentElement->next;
} }
} }
} }
// Add a collision shape to the body. // Add a collision shape to the body.
/// When you add a collision shape to the body, an internal copy of this /// When you add a collision shape to the body, an int32_ternal copy of this
/// collision shape will be created internally. Therefore, you can delete it /// collision shape will be created int32_ternally. Therefore, you can delete it
/// right after calling this method or use it later to add it to another body. /// right after calling this method or use it later to add it to another body.
/// This method will return a pointer to a new proxy shape. A proxy shape is /// This method will return a pointer to a new proxy shape. A proxy shape is
/// an object that links a collision shape and a given body. You can use the /// an object that links a collision shape and a given body. You can use the
@ -204,46 +185,46 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
/** /**
* @param collisionShape The collision shape you want to add to the body * @param collisionShape The collision shape you want to add to the body
* @param transform The transformation of the collision shape that transforms the * @param transform The transformation of the collision shape that transforms the
* local-space of the collision shape into the local-space of the body * local-space of the collision shape int32_to the local-space of the body
* @param mass Mass (in kilograms) of the collision shape you want to add * @param mass Mass (in kilograms) of the collision shape you want to add
* @return A pointer to the proxy shape that has been created to link the body to * @return A pointer to the proxy shape that has been created to link the body to
* the new collision shape you have added. * the new collision shape you have added.
*/ */
ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform, const Transform& transform,
decimal mass) { float mass) {
assert(mass > decimal(0.0)); assert(mass > float(0.0));
// Create a new proxy collision shape to attach the collision shape to the body // Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, collisionShape, sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, mass); transform, mass);
// Add it to the list of proxy collision shapes of the body // Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == NULL) { if (mProxyCollisionShapes == NULL) {
mProxyCollisionShapes = proxyShape; mProxyCollisionShapes = proxyShape;
} }
else { else {
proxyShape->mNext = mProxyCollisionShapes; proxyShape->mNext = mProxyCollisionShapes;
mProxyCollisionShapes = proxyShape; mProxyCollisionShapes = proxyShape;
} }
// Compute the world-space AABB of the new collision shape // Compute the world-space AABB of the new collision shape
AABB aabb; AABB aabb;
collisionShape->computeAABB(aabb, mTransform * transform); collisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape // Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
mNbCollisionShapes++; mNbCollisionShapes++;
// Recompute the center of mass, total mass and inertia tensor of the body with the new // Recompute the center of mass, total mass and inertia tensor of the body with the new
// collision shape // collision shape
recomputeMassInformation(); recomputeMassInformation();
// Return a pointer to the proxy collision shape // Return a pointer to the proxy collision shape
return proxyShape; return proxyShape;
} }
// Remove a collision shape from the body // Remove a collision shape from the body
@ -255,11 +236,11 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
*/ */
void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) { void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) {
// Remove the collision shape // Remove the collision shape
CollisionBody::removeCollisionShape(proxyShape); CollisionBody::removeCollisionShape(proxyShape);
// Recompute the total mass, center of mass and inertia tensor // Recompute the total mass, center of mass and inertia tensor
recomputeMassInformation(); recomputeMassInformation();
} }
// Set the linear velocity of the rigid body. // Set the linear velocity of the rigid body.
@ -268,16 +249,16 @@ void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) {
*/ */
void RigidBody::setLinearVelocity(const Vector3& linearVelocity) { void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
// If it is a static body, we do nothing // If it is a static body, we do nothing
if (mType == STATIC) return; if (mType == STATIC) return;
// Update the linear velocity of the current body state // Update the linear velocity of the current body state
mLinearVelocity = linearVelocity; mLinearVelocity = linearVelocity;
// If the linear velocity is not zero, awake the body // If the linear velocity is not zero, awake the body
if (mLinearVelocity.lengthSquare() > decimal(0.0)) { if (mLinearVelocity.lengthSquare() > float(0.0)) {
setIsSleeping(false); setIsSleeping(false);
} }
} }
// Set the angular velocity. // Set the angular velocity.
@ -286,129 +267,129 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
*/ */
void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
// If it is a static body, we do nothing // If it is a static body, we do nothing
if (mType == STATIC) return; if (mType == STATIC) return;
// Set the angular velocity // Set the angular velocity
mAngularVelocity = angularVelocity; mAngularVelocity = angularVelocity;
// If the velocity is not zero, awake the body // If the velocity is not zero, awake the body
if (mAngularVelocity.lengthSquare() > decimal(0.0)) { if (mAngularVelocity.lengthSquare() > float(0.0)) {
setIsSleeping(false); setIsSleeping(false);
} }
} }
// Set the current position and orientation // Set the current position and orientation
/** /**
* @param transform The transformation of the body that transforms the local-space * @param transform The transformation of the body that transforms the local-space
* of the body into world-space * of the body int32_to world-space
*/ */
void RigidBody::setTransform(const Transform& transform) { void RigidBody::setTransform(const Transform& transform) {
// Update the transform of the body // Update the transform of the body
mTransform = transform; mTransform = transform;
const Vector3 oldCenterOfMass = mCenterOfMassWorld; const Vector3 oldCenterOfMass = mCenterOfMassWorld;
// Compute the new center of mass in world-space coordinates // Compute the new center of mass in world-space coordinates
mCenterOfMassWorld = mTransform * mCenterOfMassLocal; mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
// Update the linear velocity of the center of mass // Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
// Update the broad-phase state of the body // Update the broad-phase state of the body
updateBroadPhaseState(); updateBroadPhaseState();
} }
// Recompute the center of mass, total mass and inertia tensor of the body using all // Recompute the center of mass, total mass and inertia tensor of the body using all
// the collision shapes attached to the body. // the collision shapes attached to the body.
void RigidBody::recomputeMassInformation() { void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0); mInitMass = float(0.0);
mMassInverse = decimal(0.0); mMassInverse = float(0.0);
mInertiaTensorLocal.setToZero(); mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero(); mInertiaTensorLocalInverse.setToZero();
mCenterOfMassLocal.setToZero(); mCenterOfMassLocal.setToZero();
// If it is STATIC or KINEMATIC body // If it is STATIC or KINEMATIC body
if (mType == STATIC || mType == KINEMATIC) { if (mType == STATIC || mType == KINEMATIC) {
mCenterOfMassWorld = mTransform.getPosition(); mCenterOfMassWorld = mTransform.getPosition();
return; return;
} }
assert(mType == DYNAMIC); assert(mType == DYNAMIC);
// Compute the total mass of the body // Compute the total mass of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
mInitMass += shape->getMass(); mInitMass += shape->getMass();
mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
} }
if (mInitMass > decimal(0.0)) { if (mInitMass > float(0.0)) {
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = float(1.0) / mInitMass;
} }
else { else {
mInitMass = decimal(1.0); mInitMass = float(1.0);
mMassInverse = decimal(1.0); mMassInverse = float(1.0);
} }
// Compute the center of mass // Compute the center of mass
const Vector3 oldCenterOfMass = mCenterOfMassWorld; const Vector3 oldCenterOfMass = mCenterOfMassWorld;
mCenterOfMassLocal *= mMassInverse; mCenterOfMassLocal *= mMassInverse;
mCenterOfMassWorld = mTransform * mCenterOfMassLocal; mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
// Compute the total mass and inertia tensor using all the collision shapes // Compute the total mass and inertia tensor using all the collision shapes
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Get the inertia tensor of the collision shape in its local-space // Get the inertia tensor of the collision shape in its local-space
Matrix3x3 inertiaTensor; Matrix3x3 inertiaTensor;
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
// Convert the collision shape inertia tensor into the local-space of the body // Convert the collision shape inertia tensor int32_to the local-space of the body
const Transform& shapeTransform = shape->getLocalToBodyTransform(); const Transform& shapeTransform = shape->getLocalToBodyTransform();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape // 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. // center int32_to a inertia tensor w.r.t to the body origin.
Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
decimal offsetSquare = offset.lengthSquare(); float offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix; Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); offsetMatrix[0].setAllValues(offsetSquare, float(0.0), float(0.0));
offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0)); offsetMatrix[1].setAllValues(float(0.0), offsetSquare, float(0.0));
offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare); offsetMatrix[2].setAllValues(float(0.0), float(0.0), offsetSquare);
offsetMatrix[0] += offset * (-offset.x); offsetMatrix[0] += offset * (-offset.x);
offsetMatrix[1] += offset * (-offset.y); offsetMatrix[1] += offset * (-offset.y);
offsetMatrix[2] += offset * (-offset.z); offsetMatrix[2] += offset * (-offset.z);
offsetMatrix *= shape->getMass(); offsetMatrix *= shape->getMass();
mInertiaTensorLocal += inertiaTensor + offsetMatrix; mInertiaTensorLocal += inertiaTensor + offsetMatrix;
} }
// Compute the local inverse inertia tensor // Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Update the linear velocity of the center of mass // Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
} }
// Update the broad-phase state for this body (because it has moved for instance) // Update the broad-phase state for this body (because it has moved for instance)
void RigidBody::updateBroadPhaseState() const { void RigidBody::updateBroadPhaseState() const {
PROFILE("RigidBody::updateBroadPhaseState()"); PROFILE("RigidBody::updateBroadPhaseState()");
DynamicsWorld& world = static_cast<DynamicsWorld&>(mWorld); DynamicsWorld& world = static_cast<DynamicsWorld&>(mWorld);
const Vector3 displacement = world.mTimeStep * mLinearVelocity; const Vector3 displacement = world.mTimeStep * mLinearVelocity;
// For all the proxy collision shapes of the body // For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Recompute the world-space AABB of the collision shape // Recompute the world-space AABB of the collision shape
AABB aabb; 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 // Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement); mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
} }
} }

404
ephysics/body/RigidBody.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_RIGID_BODY_H
#define REACTPHYSICS3D_RIGID_BODY_H
// Libraries // Libraries
#include <cassert> #include <cassert>
@ -50,193 +29,193 @@ class DynamicsWorld;
*/ */
class RigidBody : public CollisionBody { class RigidBody : public CollisionBody {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Intial mass of the body /// Intial mass of the body
decimal mInitMass; float mInitMass;
/// Center of mass of the body in local-space coordinates. /// Center of mass of the body in local-space coordinates.
/// The center of mass can therefore be different from the body origin /// The center of mass can therefore be different from the body origin
Vector3 mCenterOfMassLocal; Vector3 mCenterOfMassLocal;
/// Center of mass of the body in world-space coordinates /// Center of mass of the body in world-space coordinates
Vector3 mCenterOfMassWorld; Vector3 mCenterOfMassWorld;
/// Linear velocity of the body /// Linear velocity of the body
Vector3 mLinearVelocity; Vector3 mLinearVelocity;
/// Angular velocity of the body /// Angular velocity of the body
Vector3 mAngularVelocity; Vector3 mAngularVelocity;
/// Current external force on the body /// Current external force on the body
Vector3 mExternalForce; Vector3 mExternalForce;
/// Current external torque on the body /// Current external torque on the body
Vector3 mExternalTorque; Vector3 mExternalTorque;
/// Local inertia tensor of the body (in local-space) with respect to the /// Local inertia tensor of the body (in local-space) with respect to the
/// center of mass of the body /// center of mass of the body
Matrix3x3 mInertiaTensorLocal; Matrix3x3 mInertiaTensorLocal;
/// Inverse of the inertia tensor of the body /// Inverse of the inertia tensor of the body
Matrix3x3 mInertiaTensorLocalInverse; Matrix3x3 mInertiaTensorLocalInverse;
/// Inverse of the mass of the body /// Inverse of the mass of the body
decimal mMassInverse; float mMassInverse;
/// True if the gravity needs to be applied to this rigid body /// True if the gravity needs to be applied to this rigid body
bool mIsGravityEnabled; bool mIsGravityEnabled;
/// Material properties of the rigid body /// Material properties of the rigid body
Material mMaterial; Material mMaterial;
/// Linear velocity damping factor /// Linear velocity damping factor
decimal mLinearDamping; float mLinearDamping;
/// Angular velocity damping factor /// Angular velocity damping factor
decimal mAngularDamping; float mAngularDamping;
/// First element of the linked list of joints involving this body /// First element of the linked list of joints involving this body
JointListElement* mJointsList; JointListElement* mJointsList;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
RigidBody(const RigidBody& body); RigidBody(const RigidBody& body);
/// Private assignment operator /// Private assignment operator
RigidBody& operator=(const RigidBody& body); RigidBody& operator=(const RigidBody& body);
/// Remove a joint from the joints list /// Remove a joint from the joints list
void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
/// Update the transform of the body after a change of the center of mass /// Update the transform of the body after a change of the center of mass
void updateTransformWithCenterOfMass(); void updateTransformWithCenterOfMass();
/// Update the broad-phase state for this body (because it has moved for instance) /// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const; virtual void updateBroadPhaseState() const;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id); RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id);
/// Destructor /// Destructor
virtual ~RigidBody(); virtual ~RigidBody();
/// Set the type of the body (static, kinematic or dynamic) /// Set the type of the body (static, kinematic or dynamic)
void setType(BodyType type); void setType(BodyType type);
/// Set the current position and orientation /// Set the current position and orientation
virtual void setTransform(const Transform& transform); virtual void setTransform(const Transform& transform);
/// Return the mass of the body /// Return the mass of the body
decimal getMass() const; float getMass() const;
/// Return the linear velocity /// Return the linear velocity
Vector3 getLinearVelocity() const; Vector3 getLinearVelocity() const;
/// Set the linear velocity of the body. /// Set the linear velocity of the body.
void setLinearVelocity(const Vector3& linearVelocity); void setLinearVelocity(const Vector3& linearVelocity);
/// Return the angular velocity /// Return the angular velocity
Vector3 getAngularVelocity() const; Vector3 getAngularVelocity() const;
/// Set the angular velocity. /// Set the angular velocity.
void setAngularVelocity(const Vector3& angularVelocity); void setAngularVelocity(const Vector3& angularVelocity);
/// Set the variable to know whether or not the body is sleeping /// Set the variable to know whether or not the body is sleeping
virtual void setIsSleeping(bool isSleeping); virtual void setIsSleeping(bool isSleeping);
/// Return the local inertia tensor of the body (in body coordinates) /// Return the local inertia tensor of the body (in body coordinates)
const Matrix3x3& getInertiaTensorLocal() const; const Matrix3x3& getInertiaTensorLocal() const;
/// Set the local inertia tensor of the body (in body coordinates) /// Set the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
/// Set the local center of mass of the body (in local-space coordinates) /// Set the local center of mass of the body (in local-space coordinates)
void setCenterOfMassLocal(const Vector3& centerOfMassLocal); void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
/// Set the mass of the rigid body /// Set the mass of the rigid body
void setMass(decimal mass); void setMass(float mass);
/// Return the inertia tensor in world coordinates. /// Return the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorWorld() const; Matrix3x3 getInertiaTensorWorld() const;
/// Return the inverse of the inertia tensor in world coordinates. /// Return the inverse of the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorInverseWorld() const; Matrix3x3 getInertiaTensorInverseWorld() const;
/// Return true if the gravity needs to be applied to this rigid body /// Return true if the gravity needs to be applied to this rigid body
bool isGravityEnabled() const; bool isGravityEnabled() const;
/// Set the variable to know if the gravity is applied to this rigid body /// Set the variable to know if the gravity is applied to this rigid body
void enableGravity(bool isEnabled); void enableGravity(bool isEnabled);
/// Return a reference to the material properties of the rigid body /// Return a reference to the material properties of the rigid body
Material& getMaterial(); Material& getMaterial();
/// Set a new material for this rigid body /// Set a new material for this rigid body
void setMaterial(const Material& material); void setMaterial(const Material& material);
/// Return the linear velocity damping factor /// Return the linear velocity damping factor
decimal getLinearDamping() const; float getLinearDamping() const;
/// Set the linear damping factor /// Set the linear damping factor
void setLinearDamping(decimal linearDamping); void setLinearDamping(float linearDamping);
/// Return the angular velocity damping factor /// Return the angular velocity damping factor
decimal getAngularDamping() const; float getAngularDamping() const;
/// Set the angular damping factor /// Set the angular damping factor
void setAngularDamping(decimal angularDamping); void setAngularDamping(float angularDamping);
/// Return the first element of the linked list of joints involving this body /// Return the first element of the linked list of joints involving this body
const JointListElement* getJointsList() const; const JointListElement* getJointsList() const;
/// Return the first element of the linked list of joints involving this body /// Return the first element of the linked list of joints involving this body
JointListElement* getJointsList(); JointListElement* getJointsList();
/// Apply an external force to the body at its center of mass. /// Apply an external force to the body at its center of mass.
void applyForceToCenterOfMass(const Vector3& force); void applyForceToCenterOfMass(const Vector3& force);
/// Apply an external force to the body at a given point (in world-space coordinates). /// Apply an external force to the body at a given point (in world-space coordinates).
void applyForce(const Vector3& force, const Vector3& point); void applyForce(const Vector3& force, const Vector3& point);
/// Apply an external torque to the body. /// Apply an external torque to the body.
void applyTorque(const Vector3& torque); void applyTorque(const Vector3& torque);
/// Add a collision shape to the body. /// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
const Transform& transform, const Transform& transform,
decimal mass); float mass);
/// Remove a collision shape from the body /// Remove a collision shape from the body
virtual void removeCollisionShape(const ProxyShape* proxyShape); virtual void removeCollisionShape(const ProxyShape* proxyShape);
/// Recompute the center of mass, total mass and inertia tensor of the body using all /// Recompute the center of mass, total mass and inertia tensor of the body using all
/// the collision shapes attached to the body. /// the collision shapes attached to the body.
void recomputeMassInformation(); void recomputeMassInformation();
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;
friend class ContactSolver; friend class ContactSolver;
friend class BallAndSocketJoint; friend class BallAndSocketJoint;
friend class SliderJoint; friend class SliderJoint;
friend class HingeJoint; friend class HingeJoint;
friend class FixedJoint; friend class FixedJoint;
}; };
// Method that return the mass of the body // Method that return the mass of the body
/** /**
* @return The mass (in kilograms) of the body * @return The mass (in kilograms) of the body
*/ */
inline decimal RigidBody::getMass() const { inline float RigidBody::getMass() const {
return mInitMass; return mInitMass;
} }
// Return the linear velocity // Return the linear velocity
@ -244,7 +223,7 @@ inline decimal RigidBody::getMass() const {
* @return The linear velocity vector of the body * @return The linear velocity vector of the body
*/ */
inline Vector3 RigidBody::getLinearVelocity() const { inline Vector3 RigidBody::getLinearVelocity() const {
return mLinearVelocity; return mLinearVelocity;
} }
// Return the angular velocity of the body // Return the angular velocity of the body
@ -252,7 +231,7 @@ inline Vector3 RigidBody::getLinearVelocity() const {
* @return The angular velocity vector of the body * @return The angular velocity vector of the body
*/ */
inline Vector3 RigidBody::getAngularVelocity() const { inline Vector3 RigidBody::getAngularVelocity() const {
return mAngularVelocity; return mAngularVelocity;
} }
// Return the local inertia tensor of the body (in local-space coordinates) // Return the local inertia tensor of the body (in local-space coordinates)
@ -260,7 +239,7 @@ inline Vector3 RigidBody::getAngularVelocity() const {
* @return The 3x3 inertia tensor matrix 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 { inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
return mInertiaTensorLocal; return mInertiaTensorLocal;
} }
// Return the inertia tensor in world coordinates. // Return the inertia tensor in world coordinates.
@ -274,9 +253,9 @@ inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
*/ */
inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
// Compute and return the inertia tensor in world coordinates // Compute and return the inertia tensor in world coordinates
return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal * return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal *
mTransform.getOrientation().getMatrix().getTranspose(); mTransform.getOrientation().getMatrix().getTranspose();
} }
// Return the inverse of the inertia tensor in world coordinates. // Return the inverse of the inertia tensor in world coordinates.
@ -287,16 +266,16 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
/// current orientation quaternion of the body /// current orientation quaternion of the body
/** /**
* @return The 3x3 inverse inertia tensor matrix of the body in world-space * @return The 3x3 inverse inertia tensor matrix of the body in world-space
* coordinates * coordinates
*/ */
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE // 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 // 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 // Compute and return the inertia tensor in world coordinates
return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse * return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse *
mTransform.getOrientation().getMatrix().getTranspose(); mTransform.getOrientation().getMatrix().getTranspose();
} }
// Return true if the gravity needs to be applied to this rigid body // Return true if the gravity needs to be applied to this rigid body
@ -304,7 +283,7 @@ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
* @return True if the gravity is applied to the body * @return True if the gravity is applied to the body
*/ */
inline bool RigidBody::isGravityEnabled() const { inline bool RigidBody::isGravityEnabled() const {
return mIsGravityEnabled; return mIsGravityEnabled;
} }
// Set the variable to know if the gravity is applied to this rigid body // Set the variable to know if the gravity is applied to this rigid body
@ -312,7 +291,7 @@ inline bool RigidBody::isGravityEnabled() const {
* @param isEnabled True if you want the gravity to be applied to this body * @param isEnabled True if you want the gravity to be applied to this body
*/ */
inline void RigidBody::enableGravity(bool isEnabled) { inline void RigidBody::enableGravity(bool isEnabled) {
mIsGravityEnabled = isEnabled; mIsGravityEnabled = isEnabled;
} }
// Return a reference to the material properties of the rigid body // Return a reference to the material properties of the rigid body
@ -320,7 +299,7 @@ inline void RigidBody::enableGravity(bool isEnabled) {
* @return A reference to the material of the body * @return A reference to the material of the body
*/ */
inline Material& RigidBody::getMaterial() { inline Material& RigidBody::getMaterial() {
return mMaterial; return mMaterial;
} }
// Set a new material for this rigid body // Set a new material for this rigid body
@ -328,15 +307,15 @@ inline Material& RigidBody::getMaterial() {
* @param material The material you want to set to the body * @param material The material you want to set to the body
*/ */
inline void RigidBody::setMaterial(const Material& material) { inline void RigidBody::setMaterial(const Material& material) {
mMaterial = material; mMaterial = material;
} }
// Return the linear velocity damping factor // Return the linear velocity damping factor
/** /**
* @return The linear damping factor of this body * @return The linear damping factor of this body
*/ */
inline decimal RigidBody::getLinearDamping() const { inline float RigidBody::getLinearDamping() const {
return mLinearDamping; return mLinearDamping;
} }
// Set the linear damping factor. This is the ratio of the linear velocity // Set the linear damping factor. This is the ratio of the linear velocity
@ -344,17 +323,17 @@ inline decimal RigidBody::getLinearDamping() const {
/** /**
* @param linearDamping The linear damping factor of this body * @param linearDamping The linear damping factor of this body
*/ */
inline void RigidBody::setLinearDamping(decimal linearDamping) { inline void RigidBody::setLinearDamping(float linearDamping) {
assert(linearDamping >= decimal(0.0)); assert(linearDamping >= float(0.0));
mLinearDamping = linearDamping; mLinearDamping = linearDamping;
} }
// Return the angular velocity damping factor // Return the angular velocity damping factor
/** /**
* @return The angular damping factor of this body * @return The angular damping factor of this body
*/ */
inline decimal RigidBody::getAngularDamping() const { inline float RigidBody::getAngularDamping() const {
return mAngularDamping; return mAngularDamping;
} }
// Set the angular damping factor. This is the ratio of the angular velocity // Set the angular damping factor. This is the ratio of the angular velocity
@ -362,9 +341,9 @@ inline decimal RigidBody::getAngularDamping() const {
/** /**
* @param angularDamping The angular damping factor of this body * @param angularDamping The angular damping factor of this body
*/ */
inline void RigidBody::setAngularDamping(decimal angularDamping) { inline void RigidBody::setAngularDamping(float angularDamping) {
assert(angularDamping >= decimal(0.0)); assert(angularDamping >= float(0.0));
mAngularDamping = angularDamping; mAngularDamping = angularDamping;
} }
// Return the first element of the linked list of joints involving this body // Return the first element of the linked list of joints involving this body
@ -372,7 +351,7 @@ inline void RigidBody::setAngularDamping(decimal angularDamping) {
* @return The first element of the linked-list of all the joints involving this body * @return The first element of the linked-list of all the joints involving this body
*/ */
inline const JointListElement* RigidBody::getJointsList() const { inline const JointListElement* RigidBody::getJointsList() const {
return mJointsList; return mJointsList;
} }
// Return the first element of the linked list of joints involving this body // Return the first element of the linked list of joints involving this body
@ -380,20 +359,20 @@ inline const JointListElement* RigidBody::getJointsList() const {
* @return The first element of the linked-list of all the joints involving this body * @return The first element of the linked-list of all the joints involving this body
*/ */
inline JointListElement* RigidBody::getJointsList() { inline JointListElement* RigidBody::getJointsList() {
return mJointsList; return mJointsList;
} }
// Set the variable to know whether or not the body is sleeping // Set the variable to know whether or not the body is sleeping
inline void RigidBody::setIsSleeping(bool isSleeping) { inline void RigidBody::setIsSleeping(bool isSleeping) {
if (isSleeping) { if (isSleeping) {
mLinearVelocity.setToZero(); mLinearVelocity.setToZero();
mAngularVelocity.setToZero(); mAngularVelocity.setToZero();
mExternalForce.setToZero(); mExternalForce.setToZero();
mExternalTorque.setToZero(); mExternalTorque.setToZero();
} }
Body::setIsSleeping(isSleeping); Body::setIsSleeping(isSleeping);
} }
// Apply an external force to the body at its center of mass. // Apply an external force to the body at its center of mass.
@ -406,16 +385,16 @@ inline void RigidBody::setIsSleeping(bool isSleeping) {
*/ */
inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) { inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
// If it is not a dynamic body, we do nothing // If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
// Awake the body if it was sleeping // Awake the body if it was sleeping
if (mIsSleeping) { if (mIsSleeping) {
setIsSleeping(false); setIsSleeping(false);
} }
// Add the force // Add the force
mExternalForce += force; mExternalForce += force;
} }
// Apply an external force to the body at a given point (in world-space coordinates). // Apply an external force to the body at a given point (in world-space coordinates).
@ -431,17 +410,17 @@ inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
*/ */
inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) { inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
// If it is not a dynamic body, we do nothing // If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
// Awake the body if it was sleeping // Awake the body if it was sleeping
if (mIsSleeping) { if (mIsSleeping) {
setIsSleeping(false); setIsSleeping(false);
} }
// Add the force and torque // Add the force and torque
mExternalForce += force; mExternalForce += force;
mExternalTorque += (point - mCenterOfMassWorld).cross(force); mExternalTorque += (point - mCenterOfMassWorld).cross(force);
} }
// Apply an external torque to the body. // Apply an external torque to the body.
@ -454,25 +433,24 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
*/ */
inline void RigidBody::applyTorque(const Vector3& torque) { inline void RigidBody::applyTorque(const Vector3& torque) {
// If it is not a dynamic body, we do nothing // If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
// Awake the body if it was sleeping // Awake the body if it was sleeping
if (mIsSleeping) { if (mIsSleeping) {
setIsSleeping(false); setIsSleeping(false);
} }
// Add the torque // Add the torque
mExternalTorque += torque; mExternalTorque += torque;
} }
/// Update the transform of the body after a change of the center of mass /// Update the transform of the body after a change of the center of mass
inline void RigidBody::updateTransformWithCenterOfMass() { inline void RigidBody::updateTransformWithCenterOfMass() {
// Translate the body according to the translation of the center of mass position // Translate the body according to the translation of the center of mass position
mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal); mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal);
} }
} }
#endif

673
ephysics/collision/CollisionDetection.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/CollisionDetection.h> #include <ephysics/collision/CollisionDetection.h>
@ -42,15 +23,15 @@ using namespace std;
// Constructor // Constructor
CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator) CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator)
: mMemoryAllocator(memoryAllocator), : mMemoryAllocator(memoryAllocator),
mWorld(world), mBroadPhaseAlgorithm(*this), mWorld(world), mBroadPhaseAlgorithm(*this),
mIsCollisionShapesAdded(false) { mIsCollisionShapesAdded(false) {
// Set the default collision dispatch configuration // Set the default collision dispatch configuration
setCollisionDispatch(&mDefaultCollisionDispatch); setCollisionDispatch(&mDefaultCollisionDispatch);
// Fill-in the collision detection matrix with algorithms // Fill-in the collision detection matrix with algorithms
fillInCollisionMatrix(); fillInCollisionMatrix();
} }
// Destructor // Destructor
@ -61,458 +42,458 @@ CollisionDetection::~CollisionDetection() {
// Compute the collision detection // Compute the collision detection
void CollisionDetection::computeCollisionDetection() { void CollisionDetection::computeCollisionDetection() {
PROFILE("CollisionDetection::computeCollisionDetection()"); PROFILE("CollisionDetection::computeCollisionDetection()");
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
computeNarrowPhase(); computeNarrowPhase();
} }
// Compute the collision detection // Compute the collision detection
void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2) { const std::set<uint32_t>& shapes2) {
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
// Delete all the contact points in the currently overlapping pairs // Delete all the contact points in the currently overlapping pairs
clearContactPoints(); clearContactPoints();
// Compute the narrow-phase collision detection among given sets of shapes // Compute the narrow-phase collision detection among given sets of shapes
computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
} }
// Report collision between two sets of shapes // Report collision between two sets of shapes
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2) { const std::set<uint32_t>& shapes2) {
// For each possible collision pair of bodies // For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it; map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it->second;
const ProxyShape* shape1 = pair->getShape1(); const ProxyShape* shape1 = pair->getShape1();
const ProxyShape* shape2 = pair->getShape2(); const ProxyShape* shape2 = pair->getShape2();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that // If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one // shape1 is among on set and shape2 is among the other one
if (!shapes1.empty() && !shapes2.empty() && if (!shapes1.empty() && !shapes2.empty() &&
(shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
(shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
continue; continue;
} }
if (!shapes1.empty() && shapes2.empty() && if (!shapes1.empty() && shapes2.empty() &&
shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
{ {
continue; continue;
} }
if (!shapes2.empty() && shapes1.empty() && if (!shapes2.empty() && shapes1.empty() &&
shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
{ {
continue; continue;
} }
// For each contact manifold set of the overlapping pair // For each contact manifold set of the overlapping pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) { for (int32_t j=0; j<manifoldSet.getNbContactManifolds(); j++) {
const ContactManifold* manifold = manifoldSet.getContactManifold(j); const ContactManifold* manifold = manifoldSet.getContactManifold(j);
// For each contact manifold of the manifold set // For each contact manifold of the manifold set
for (uint i=0; i<manifold->getNbContactPoints(); i++) { for (uint32_t i=0; i<manifold->getNbContactPoints(); i++) {
ContactPoint* contactPoint = manifold->getContactPoint(i); ContactPoint* contactPoint = manifold->getContactPoint(i);
// Create the contact info object for the contact // Create the contact info object for the contact
ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(), ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(),
manifold->getShape1()->getCollisionShape(), manifold->getShape1()->getCollisionShape(),
manifold->getShape2()->getCollisionShape(), manifold->getShape2()->getCollisionShape(),
contactPoint->getNormal(), contactPoint->getNormal(),
contactPoint->getPenetrationDepth(), contactPoint->getPenetrationDepth(),
contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody1(),
contactPoint->getLocalPointOnBody2()); contactPoint->getLocalPointOnBody2());
// Notify the collision callback about this new contact // Notify the collision callback about this new contact
if (callback != NULL) callback->notifyContact(contactInfo); if (callback != NULL) callback->notifyContact(contactInfo);
} }
} }
} }
} }
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() { void CollisionDetection::computeBroadPhase() {
PROFILE("CollisionDetection::computeBroadPhase()"); PROFILE("CollisionDetection::computeBroadPhase()");
// If new collision shapes have been added to bodies // If new collision shapes have been added to bodies
if (mIsCollisionShapesAdded) { if (mIsCollisionShapesAdded) {
// Ask the broad-phase to recompute the overlapping pairs of collision // Ask the broad-phase to recompute the overlapping pairs of collision
// shapes. This call can only add new overlapping pairs in the collision // shapes. This call can only add new overlapping pairs in the collision
// detection. // detection.
mBroadPhaseAlgorithm.computeOverlappingPairs(); mBroadPhaseAlgorithm.computeOverlappingPairs();
} }
} }
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhase() { void CollisionDetection::computeNarrowPhase() {
PROFILE("CollisionDetection::computeNarrowPhase()"); PROFILE("CollisionDetection::computeNarrowPhase()");
// Clear the set of overlapping pairs in narrow-phase contact // Clear the set of overlapping pairs in narrow-phase contact
mContactOverlappingPairs.clear(); mContactOverlappingPairs.clear();
// For each possible collision pair of bodies // For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it; map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it->second;
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair->getShape2();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// Check if the collision filtering allows collision between the two shapes and // Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the // that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair // overlapping pair
if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it; std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it; ++it;
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
itToRemove->second->~OverlappingPair(); itToRemove->second->~OverlappingPair();
mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove); mOverlappingPairs.erase(itToRemove);
continue; continue;
} }
else { else {
++it; ++it;
} }
CollisionBody* const body1 = shape1->getBody(); CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody(); CollisionBody* const body2 = shape2->getBody();
// Update the contact cache of the overlapping pair // Update the contact cache of the overlapping pair
pair->update(); pair->update();
// Check that at least one body is awake and not static // Check that at least one body is awake and not static
bool isBody1Active = !body1->isSleeping() && body1->getType() != STATIC; bool isBody1Active = !body1->isSleeping() && body1->getType() != STATIC;
bool isBody2Active = !body2->isSleeping() && body2->getType() != STATIC; bool isBody2Active = !body2->isSleeping() && body2->getType() != STATIC;
if (!isBody1Active && !isBody2Active) continue; if (!isBody1Active && !isBody2Active) continue;
// Check if the bodies are in the set of bodies that cannot collide between each other // Check if the bodies are in the set of bodies that cannot collide between each other
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type]; NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type];
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == NULL) continue; if (narrowPhaseAlgorithm == NULL) continue;
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData()); pair, shape1->getCachedCollisionData());
CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
pair, shape2->getCachedCollisionData()); pair, shape2->getCachedCollisionData());
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this); narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this);
} }
// Add all the contact manifolds (between colliding bodies) to the bodies // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
} }
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2) { const std::set<uint32_t>& shapes2) {
mContactOverlappingPairs.clear(); mContactOverlappingPairs.clear();
// For each possible collision pair of bodies // For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it; map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it->second;
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair->getShape2();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that // If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one // shape1 is among on set and shape2 is among the other one
if (!shapes1.empty() && !shapes2.empty() && if (!shapes1.empty() && !shapes2.empty() &&
(shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
(shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
++it; ++it;
continue; continue;
} }
if (!shapes1.empty() && shapes2.empty() && if (!shapes1.empty() && shapes2.empty() &&
shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
{ {
++it; ++it;
continue; continue;
} }
if (!shapes2.empty() && shapes1.empty() && if (!shapes2.empty() && shapes1.empty() &&
shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
{ {
++it; ++it;
continue; continue;
} }
// Check if the collision filtering allows collision between the two shapes and // Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the // that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair // overlapping pair
if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it; std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it; ++it;
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
itToRemove->second->~OverlappingPair(); itToRemove->second->~OverlappingPair();
mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove); mOverlappingPairs.erase(itToRemove);
continue; continue;
} }
else { else {
++it; ++it;
} }
CollisionBody* const body1 = shape1->getBody(); CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody(); CollisionBody* const body2 = shape2->getBody();
// Update the contact cache of the overlapping pair // Update the contact cache of the overlapping pair
pair->update(); pair->update();
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision // Check if the two bodies are allowed to collide, otherwise, we do not test for collision
if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue; if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue;
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
// Check if the two bodies are sleeping, if so, we do no test collision between them // Check if the two bodies are sleeping, if so, we do no test collision between them
if (body1->isSleeping() && body2->isSleeping()) continue; if (body1->isSleeping() && body2->isSleeping()) continue;
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type]; NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type];
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == NULL) continue; if (narrowPhaseAlgorithm == NULL) continue;
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData()); pair, shape1->getCachedCollisionData());
CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
pair, shape2->getCachedCollisionData()); pair, shape2->getCachedCollisionData());
TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision // if there really is a collision
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback); narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback);
} }
// Add all the contact manifolds (between colliding bodies) to the bodies // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
} }
// Allow the broadphase to notify the collision detection about an overlapping pair. // Allow the broadphase to notify the collision detection about an overlapping pair.
/// This method is called by the broad-phase collision detection algorithm /// This method is called by the broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// If the two proxy collision shapes are from the same body, skip it // If the two proxy collision shapes are from the same body, skip it
if (shape1->getBody()->getID() == shape2->getBody()->getID()) return; if (shape1->getBody()->getID() == shape2->getBody()->getID()) return;
// Check if the collision filtering allows collision between the two shapes // Check if the collision filtering allows collision between the two shapes
if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
// Compute the overlapping pair ID // Compute the overlapping pair ID
overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
// Check if the overlapping pair already exists // Check if the overlapping pair already exists
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
// Compute the maximum number of contact manifolds for this pair // Compute the maximum number of contact manifolds for this pair
int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), int32_t nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
shape2->getCollisionShape()->getType()); shape2->getCollisionShape()->getType());
// Create the overlapping pair and add it into the set of overlapping pairs // Create the overlapping pair and add it int32_to the set of overlapping pairs
OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair))) OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator); OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator);
assert(newPair != NULL); assert(newPair != NULL);
#ifndef NDEBUG #ifndef NDEBUG
std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check = std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
#endif #endif
mOverlappingPairs.insert(make_pair(pairID, newPair)); mOverlappingPairs.insert(make_pair(pairID, newPair));
assert(check.second); assert(check.second);
// Wake up the two bodies // Wake up the two bodies
shape1->getBody()->setIsSleeping(false); shape1->getBody()->setIsSleeping(false);
shape2->getBody()->setIsSleeping(false); shape2->getBody()->setIsSleeping(false);
} }
// Remove a body from the collision detection // Remove a body from the collision detection
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Remove all the overlapping pairs involving this proxy shape // Remove all the overlapping pairs involving this proxy shape
std::map<overlappingpairid, OverlappingPair*>::iterator it; std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID|| if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID||
it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) { it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it; std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it; ++it;
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
itToRemove->second->~OverlappingPair(); itToRemove->second->~OverlappingPair();
mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove); mOverlappingPairs.erase(itToRemove);
} }
else { else {
++it; ++it;
} }
} }
// Remove the body from the broad-phase // Remove the body from the broad-phase
mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
} }
// Called by a narrow-phase collision algorithm when a new contact has been found // Called by a narrow-phase collision algorithm when a new contact has been found
void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) {
// If it is the first contact since the pairs are overlapping // If it is the first contact since the pairs are overlapping
if (overlappingPair->getNbContactPoints() == 0) { if (overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event // Trigger a callback event
if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(contactInfo); if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(contactInfo);
} }
// Create a new contact // Create a new contact
createContact(overlappingPair, contactInfo); createContact(overlappingPair, contactInfo);
// Trigger a callback event for the new contact // Trigger a callback event for the new contact
if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(contactInfo); if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(contactInfo);
} }
// Create a new contact // Create a new contact
void CollisionDetection::createContact(OverlappingPair* overlappingPair, void CollisionDetection::createContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) { const ContactPointInfo& contactInfo) {
// Create a new contact // Create a new contact
ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactInfo); ContactPoint(contactInfo);
// Add the contact to the contact manifold set of the corresponding overlapping pair // Add the contact to the contact manifold set of the corresponding overlapping pair
overlappingPair->addContact(contact); overlappingPair->addContact(contact);
// Add the overlapping pair into the set of pairs in contact during narrow-phase // Add the overlapping pair int32_to the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
overlappingPair->getShape2()); overlappingPair->getShape2());
mContactOverlappingPairs[pairId] = overlappingPair; mContactOverlappingPairs[pairId] = overlappingPair;
} }
void CollisionDetection::addAllContactManifoldsToBodies() { void CollisionDetection::addAllContactManifoldsToBodies() {
// For each overlapping pairs in contact during the narrow-phase // For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it; std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// Add all the contact manifolds of the pair into the list of contact manifolds // Add all the contact manifolds of the pair int32_to the list of contact manifolds
// of the two bodies involved in the contact // of the two bodies involved in the contact
addContactManifoldToBody(it->second); addContactManifoldToBody(it->second);
} }
} }
// Add a contact manifold to the linked list of contact manifolds of the two bodies involved // Add a contact manifold to the linked list of contact manifolds of the two bodies involved
// in the corresponding contact // in the corresponding contact
void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
assert(pair != NULL); assert(pair != NULL);
CollisionBody* body1 = pair->getShape1()->getBody(); CollisionBody* body1 = pair->getShape1()->getBody();
CollisionBody* body2 = pair->getShape2()->getBody(); CollisionBody* body2 = pair->getShape2()->getBody();
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair // For each contact manifold in the set of manifolds in the pair
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) { for (int32_t i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* contactManifold = manifoldSet.getContactManifold(i); ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
assert(contactManifold->getNbContactPoints() > 0); assert(contactManifold->getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked // Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body // list of contact manifolds of the first body
void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1) ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold, ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList); body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1; body1->mContactManifoldsList = listElement1;
// Add the contact manifold at the beginning of the linked // Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body // list of the contact manifolds of the second body
void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2) ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold, ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList); body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2; body2->mContactManifoldsList = listElement2;
} }
} }
// Delete all the contact points in the currently overlapping pairs // Delete all the contact points in the currently overlapping pairs
void CollisionDetection::clearContactPoints() { void CollisionDetection::clearContactPoints() {
// For each overlapping pair // For each overlapping pair
std::map<overlappingpairid, OverlappingPair*>::iterator it; std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
it->second->clearContactPoints(); it->second->clearContactPoints();
} }
} }
// Fill-in the collision detection matrix // Fill-in the collision detection matrix
void CollisionDetection::fillInCollisionMatrix() { void CollisionDetection::fillInCollisionMatrix() {
// For each possible type of collision shape // For each possible type of collision shape
for (int i=0; i<NB_COLLISION_SHAPE_TYPES; i++) { for (int32_t i=0; i<NB_COLLISION_SHAPE_TYPES; i++) {
for (int j=0; j<NB_COLLISION_SHAPE_TYPES; j++) { for (int32_t j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
mCollisionMatrix[i][j] = mCollisionDispatch->selectAlgorithm(i, j); mCollisionMatrix[i][j] = mCollisionDispatch->selectAlgorithm(i, j);
} }
} }
} }
// Return the world event listener // Return the world event listener
@ -527,6 +508,6 @@ MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() {
// Called by a narrow-phase collision algorithm when a new contact has been found // Called by a narrow-phase collision algorithm when a new contact has been found
void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) { const ContactPointInfo& contactInfo) {
mCollisionCallback->notifyContact(contactInfo); mCollisionCallback->notifyContact(contactInfo);
} }

329
ephysics/collision/CollisionDetection.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_DETECTION_H
#define REACTPHYSICS3D_COLLISION_DETECTION_H
// Libraries // Libraries
#include <ephysics/body/CollisionBody.h> #include <ephysics/body/CollisionBody.h>
@ -50,21 +29,21 @@ class CollisionCallback;
// Class TestCollisionBetweenShapesCallback // Class TestCollisionBetweenShapesCallback
class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback { class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
private: private:
CollisionCallback* mCollisionCallback; CollisionCallback* mCollisionCallback;
public: public:
// Constructor // Constructor
TestCollisionBetweenShapesCallback(CollisionCallback* callback) TestCollisionBetweenShapesCallback(CollisionCallback* callback)
: mCollisionCallback(callback) { : mCollisionCallback(callback) {
} }
// Called by a narrow-phase collision algorithm when a new contact has been found // Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo); const ContactPointInfo& contactInfo);
}; };
// Class CollisionDetection // Class CollisionDetection
@ -76,243 +55,241 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
*/ */
class CollisionDetection : public NarrowPhaseCallback { class CollisionDetection : public NarrowPhaseCallback {
private : private :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Collision Detection Dispatch configuration /// Collision Detection Dispatch configuration
CollisionDispatch* mCollisionDispatch; CollisionDispatch* mCollisionDispatch;
/// Default collision dispatch configuration /// Default collision dispatch configuration
DefaultCollisionDispatch mDefaultCollisionDispatch; DefaultCollisionDispatch mDefaultCollisionDispatch;
/// Collision detection matrix (algorithms to use) /// Collision detection matrix (algorithms to use)
NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
/// Reference to the memory allocator /// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator; MemoryAllocator& mMemoryAllocator;
/// Pointer to the physics world /// Pointer to the physics world
CollisionWorld* mWorld; CollisionWorld* mWorld;
/// Broad-phase overlapping pairs /// Broad-phase overlapping pairs
std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs; std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
/// Overlapping pairs in contact (during the current Narrow-phase collision detection) /// Overlapping pairs in contact (during the current Narrow-phase collision detection)
std::map<overlappingpairid, OverlappingPair*> mContactOverlappingPairs; std::map<overlappingpairid, OverlappingPair*> mContactOverlappingPairs;
/// Broad-phase algorithm /// Broad-phase algorithm
BroadPhaseAlgorithm mBroadPhaseAlgorithm; BroadPhaseAlgorithm mBroadPhaseAlgorithm;
/// Narrow-phase GJK algorithm /// Narrow-phase GJK algorithm
// TODO : Delete this // TODO : Delete this
GJKAlgorithm mNarrowPhaseGJKAlgorithm; GJKAlgorithm mNarrowPhaseGJKAlgorithm;
/// Set of pair of bodies that cannot collide between each other /// Set of pair of bodies that cannot collide between each other
std::set<bodyindexpair> mNoCollisionPairs; std::set<bodyindexpair> mNoCollisionPairs;
/// True if some collision shapes have been added previously /// True if some collision shapes have been added previously
bool mIsCollisionShapesAdded; bool mIsCollisionShapesAdded;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
CollisionDetection(const CollisionDetection& collisionDetection); CollisionDetection(const CollisionDetection& collisionDetection);
/// Private assignment operator /// Private assignment operator
CollisionDetection& operator=(const CollisionDetection& collisionDetection); CollisionDetection& operator=(const CollisionDetection& collisionDetection);
/// Compute the broad-phase collision detection /// Compute the broad-phase collision detection
void computeBroadPhase(); void computeBroadPhase();
/// Compute the narrow-phase collision detection /// Compute the narrow-phase collision detection
void computeNarrowPhase(); void computeNarrowPhase();
/// Add a contact manifold to the linked list of contact manifolds of the two bodies /// Add a contact manifold to the linked list of contact manifolds of the two bodies
/// involed in the corresponding contact. /// involed in the corresponding contact.
void addContactManifoldToBody(OverlappingPair* pair); void addContactManifoldToBody(OverlappingPair* pair);
/// Delete all the contact points in the currently overlapping pairs /// Delete all the contact points in the currently overlapping pairs
void clearContactPoints(); void clearContactPoints();
/// Fill-in the collision detection matrix /// Fill-in the collision detection matrix
void fillInCollisionMatrix(); void fillInCollisionMatrix();
/// Add all the contact manifold of colliding pairs to their bodies /// Add all the contact manifold of colliding pairs to their bodies
void addAllContactManifoldsToBodies(); void addAllContactManifoldsToBodies();
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator); CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator);
/// Destructor /// Destructor
~CollisionDetection(); ~CollisionDetection();
/// Set the collision dispatch configuration /// Set the collision dispatch configuration
void setCollisionDispatch(CollisionDispatch* collisionDispatch); void setCollisionDispatch(CollisionDispatch* collisionDispatch);
/// Return the Narrow-phase collision detection algorithm to use between two types of shapes /// Return the Narrow-phase collision detection algorithm to use between two types of shapes
NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type, NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type,
CollisionShapeType shape2Type) const; CollisionShapeType shape2Type) const;
/// Add a proxy collision shape to the collision detection /// Add a proxy collision shape to the collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
/// Remove a proxy collision shape from the collision detection /// Remove a proxy collision shape from the collision detection
void removeProxyCollisionShape(ProxyShape* proxyShape); void removeProxyCollisionShape(ProxyShape* proxyShape);
/// Update a proxy collision shape (that has moved for instance) /// Update a proxy collision shape (that has moved for instance)
void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const Vector3& displacement = Vector3(0, 0, 0), bool forceReinsert = false); const Vector3& displacement = Vector3(0, 0, 0), bool forceReinsert = false);
/// Add a pair of bodies that cannot collide with each other /// Add a pair of bodies that cannot collide with each other
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
/// Remove a pair of bodies that cannot collide with each other /// Remove a pair of bodies that cannot collide with each other
void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2); void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
/// Ask for a collision shape to be tested again during broad-phase. /// Ask for a collision shape to be tested again during broad-phase.
void askForBroadPhaseCollisionCheck(ProxyShape* shape); void askForBroadPhaseCollisionCheck(ProxyShape* shape);
/// Compute the collision detection /// Compute the collision detection
void computeCollisionDetection(); void computeCollisionDetection();
/// Compute the collision detection /// Compute the collision detection
void testCollisionBetweenShapes(CollisionCallback* callback, void testCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2); const std::set<uint32_t>& shapes2);
/// Report collision between two sets of shapes /// Report collision between two sets of shapes
void reportCollisionBetweenShapes(CollisionCallback* callback, void reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2) ; const std::set<uint32_t>& shapes2) ;
/// Ray casting method /// Ray casting method
void raycast(RaycastCallback* raycastCallback, const Ray& ray, void raycast(RaycastCallback* raycastCallback, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const; unsigned short raycastWithCategoryMaskBits) const;
/// Test if the AABBs of two bodies overlap /// Test if the AABBs of two bodies overlap
bool testAABBOverlap(const CollisionBody* body1, bool testAABBOverlap(const CollisionBody* body1,
const CollisionBody* body2) const; const CollisionBody* body2) const;
/// Test if the AABBs of two proxy shapes overlap /// Test if the AABBs of two proxy shapes overlap
bool testAABBOverlap(const ProxyShape* shape1, bool testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const; const ProxyShape* shape2) const;
/// Allow the broadphase to notify the collision detection about an overlapping pair. /// Allow the broadphase to notify the collision detection about an overlapping pair.
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
/// Compute the narrow-phase collision detection /// Compute the narrow-phase collision detection
void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2); const std::set<uint32_t>& shapes2);
/// Return a pointer to the world /// Return a pointer to the world
CollisionWorld* getWorld(); CollisionWorld* getWorld();
/// Return the world event listener /// Return the world event listener
EventListener* getWorldEventListener(); EventListener* getWorldEventListener();
/// Return a reference to the world memory allocator /// Return a reference to the world memory allocator
MemoryAllocator& getWorldMemoryAllocator(); MemoryAllocator& getWorldMemoryAllocator();
/// Called by a narrow-phase collision algorithm when a new contact has been found /// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo); virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
/// Create a new contact /// Create a new contact
void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo); void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;
friend class ConvexMeshShape; friend class ConvexMeshShape;
}; };
// Return the Narrow-phase collision detection algorithm to use between two types of shapes // Return the Narrow-phase collision detection algorithm to use between two types of shapes
inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type,
CollisionShapeType shape2Type) const { CollisionShapeType shape2Type) const {
return mCollisionMatrix[shape1Type][shape2Type]; return mCollisionMatrix[shape1Type][shape2Type];
} }
// Set the collision dispatch configuration // Set the collision dispatch configuration
inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDispatch = collisionDispatch; mCollisionDispatch = collisionDispatch;
mCollisionDispatch->init(this, &mMemoryAllocator); mCollisionDispatch->init(this, &mMemoryAllocator);
// Fill-in the collision matrix with the new algorithms to use // Fill-in the collision matrix with the new algorithms to use
fillInCollisionMatrix(); fillInCollisionMatrix();
} }
// Add a body to the collision detection // Add a body to the collision detection
inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape, inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
const AABB& aabb) { const AABB& aabb) {
// Add the body to the broad-phase // Add the body to the broad-phase
mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb);
mIsCollisionShapesAdded = true; mIsCollisionShapesAdded = true;
} }
// Add a pair of bodies that cannot collide with each other // Add a pair of bodies that cannot collide with each other
inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1, inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) { CollisionBody* body2) {
mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2)); mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2));
} }
// Remove a pair of bodies that cannot collide with each other // Remove a pair of bodies that cannot collide with each other
inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) { CollisionBody* body2) {
mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2));
} }
// Ask for a collision shape to be tested again during broad-phase. // 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 /// 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. /// previous frame so that it is tested for collision again in the broad-phase.
inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
} }
// Update a proxy collision shape (that has moved for instance) // Update a proxy collision shape (that has moved for instance)
inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert) { const Vector3& displacement, bool forceReinsert) {
mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
} }
// Ray casting method // Ray casting method
inline void CollisionDetection::raycast(RaycastCallback* raycastCallback, inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const { unsigned short raycastWithCategoryMaskBits) const {
PROFILE("CollisionDetection::raycast()"); PROFILE("CollisionDetection::raycast()");
RaycastTest rayCastTest(raycastCallback); RaycastTest rayCastTest(raycastCallback);
// Ask the broad-phase algorithm to call the testRaycastAgainstShape() // Ask the broad-phase algorithm to call the testRaycastAgainstShape()
// callback method for each proxy shape hit by the ray in the broad-phase // callback method for each proxy shape hit by the ray in the broad-phase
mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
} }
// Test if the AABBs of two proxy shapes overlap // Test if the AABBs of two proxy shapes overlap
inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1, inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const { const ProxyShape* shape2) const {
// If one of the shape's body is not active, we return no overlap // If one of the shape's body is not active, we return no overlap
if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) { if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) {
return false; return false;
} }
return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
} }
// Return a pointer to the world // Return a pointer to the world
inline CollisionWorld* CollisionDetection::getWorld() { inline CollisionWorld* CollisionDetection::getWorld() {
return mWorld; return mWorld;
} }
} }
#endif

72
ephysics/collision/CollisionShapeInfo.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SHAPE_INFO_H
#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
// Libraries // Libraries
#include <ephysics/collision/shapes/CollisionShape.h> #include <ephysics/collision/shapes/CollisionShape.h>
@ -41,35 +20,34 @@ class OverlappingPair;
*/ */
struct CollisionShapeInfo { struct CollisionShapeInfo {
public: public:
/// Broadphase overlapping pair /// Broadphase overlapping pair
OverlappingPair* overlappingPair; OverlappingPair* overlappingPair;
/// Proxy shape /// Proxy shape
ProxyShape* proxyShape; ProxyShape* proxyShape;
/// Pointer to the collision shape /// Pointer to the collision shape
const CollisionShape* collisionShape; const CollisionShape* collisionShape;
/// Transform that maps from collision shape local-space to world-space /// Transform that maps from collision shape local-space to world-space
Transform shapeToWorldTransform; Transform shapeToWorldTransform;
/// Cached collision data of the proxy shape /// Cached collision data of the proxy shape
void** cachedCollisionData; void** cachedCollisionData;
/// Constructor /// Constructor
CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape, CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape,
const Transform& shapeLocalToWorldTransform, OverlappingPair* pair, const Transform& shapeLocalToWorldTransform, OverlappingPair* pair,
void** cachedData) void** cachedData)
: overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape), : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape),
shapeToWorldTransform(shapeLocalToWorldTransform), shapeToWorldTransform(shapeLocalToWorldTransform),
cachedCollisionData(cachedData) { cachedCollisionData(cachedData) {
} }
}; };
} }
#endif

344
ephysics/collision/ContactManifold.cpp
View File

@ -1,27 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <iostream> #include <iostream>
@ -31,71 +13,71 @@ using namespace reactphysics3d;
// Constructor // Constructor
ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short normalDirectionId) MemoryAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) { mMemoryAllocator(memoryAllocator) {
} }
// Destructor // Destructor
ContactManifold::~ContactManifold() { ContactManifold::~ContactManifold() {
clear(); clear();
} }
// Add a contact point in the manifold // Add a contact point in the manifold
void ContactManifold::addContactPoint(ContactPoint* contact) { void ContactManifold::addContactPoint(ContactPoint* contact) {
// For contact already in the manifold // For contact already in the manifold
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point // Check if the new point point does not correspond to a same contact point
// already in the manifold. // already in the manifold.
decimal distance = (mContactPoints[i]->getWorldPointOnBody1() - float distance = (mContactPoints[i]->getWorldPointOnBody1() -
contact->getWorldPointOnBody1()).lengthSquare(); contact->getWorldPointOnBody1()).lengthSquare();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
// Delete the new contact // Delete the new contact
contact->~ContactPoint(); contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint)); mMemoryAllocator.release(contact, sizeof(ContactPoint));
assert(mNbContactPoints > 0); assert(mNbContactPoints > 0);
return; return;
} }
} }
// If the contact manifold is full // If the contact manifold is full
if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int32_t indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); int32_t indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove); removeContactPoint(indexToRemove);
} }
// Add the new contact point in the manifold // Add the new contact point in the manifold
mContactPoints[mNbContactPoints] = contact; mContactPoints[mNbContactPoints] = contact;
mNbContactPoints++; mNbContactPoints++;
assert(mNbContactPoints > 0); assert(mNbContactPoints > 0);
} }
// Remove a contact point from the manifold // Remove a contact point from the manifold
void ContactManifold::removeContactPoint(uint index) { void ContactManifold::removeContactPoint(uint32_t index) {
assert(index < mNbContactPoints); assert(index < mNbContactPoints);
assert(mNbContactPoints > 0); assert(mNbContactPoints > 0);
// Call the destructor explicitly and tell the memory allocator that // Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free // the corresponding memory block is now free
mContactPoints[index]->~ContactPoint(); mContactPoints[index]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint)); mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
// If we don't remove the last index // If we don't remove the last index
if (index < mNbContactPoints - 1) { if (index < mNbContactPoints - 1) {
mContactPoints[index] = mContactPoints[mNbContactPoints - 1]; mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
} }
mNbContactPoints--; mNbContactPoints--;
} }
// Update the contact manifold // Update the contact manifold
@ -106,68 +88,68 @@ void ContactManifold::removeContactPoint(uint index) {
/// contact normal. /// contact normal.
void ContactManifold::update(const Transform& transform1, const Transform& transform2) { void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
if (mNbContactPoints == 0) return; if (mNbContactPoints == 0) return;
// Update the world coordinates and penetration depth of the contact points in the manifold // Update the world coordinates and penetration depth of the contact points in the manifold
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
mContactPoints[i]->setWorldPointOnBody1(transform1 * mContactPoints[i]->setWorldPointOnBody1(transform1 *
mContactPoints[i]->getLocalPointOnBody1()); mContactPoints[i]->getLocalPointOnBody1());
mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->setWorldPointOnBody2(transform2 *
mContactPoints[i]->getLocalPointOnBody2()); mContactPoints[i]->getLocalPointOnBody2());
mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() -
mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
} }
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * const float squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD; PERSISTENT_CONTACT_DIST_THRESHOLD;
// Remove the contact points that don't represent very well the contact manifold // Remove the contact points that don't represent very well the contact manifold
for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) { for (int32_t i=static_cast<int32_t>(mNbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int>(mNbContactPoints)); assert(i < static_cast<int32_t>(mNbContactPoints));
// Compute the distance between contact points in the normal direction // Compute the distance between contact points in the normal direction
decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth(); float distanceNormal = -mContactPoints[i]->getPenetrationDepth();
// If the contacts points are too far from each other in the normal direction // If the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) { if (distanceNormal > squarePersistentContactThreshold) {
removeContactPoint(i); removeContactPoint(i);
} }
else { else {
// Compute the distance of the two contact points in the plane // Compute the distance of the two contact points in the plane
// orthogonal to the contact normal // orthogonal to the contact normal
Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() + Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
mContactPoints[i]->getNormal() * distanceNormal; mContactPoints[i]->getNormal() * distanceNormal;
Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1; Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance // If the orthogonal distance is larger than the valid distance
// threshold, we remove the contact // threshold, we remove the contact
if (projDifference.lengthSquare() > squarePersistentContactThreshold) { if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
removeContactPoint(i); removeContactPoint(i);
} }
} }
} }
} }
// Return the index of the contact point with the larger penetration depth. // 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 /// This corresponding contact will be kept in the cache. The method returns -1 is
/// the new contact is the deepest. /// the new contact is the deepest.
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { int32_t ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
int indexMaxPenetrationDepth = -1; int32_t indexMaxPenetrationDepth = -1;
decimal maxPenetrationDepth = newContact->getPenetrationDepth(); float maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache // For each contact in the cache
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
// If the current contact has a larger penetration depth // If the current contact has a larger penetration depth
if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) { if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth(); maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i; indexMaxPenetrationDepth = i;
} }
} }
// Return the index of largest penetration depth // Return the index of largest penetration depth
return indexMaxPenetrationDepth; return indexMaxPenetrationDepth;
} }
// Return the index that will be removed. // Return the index that will be removed.
@ -180,84 +162,84 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) cons
/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, /// 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 /// 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). /// by Erwin Coumans (http://wwww.bulletphysics.org).
int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint float area0 = 0.0; // Area with contact 1,2,3 and newPoint
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint float area1 = 0.0; // Area with contact 0,2,3 and newPoint
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint float area2 = 0.0; // Area with contact 0,1,3 and newPoint
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint float area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) { if (indexMaxPenetration != 0) {
// Compute the area // Compute the area
Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1(); mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2); Vector3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.lengthSquare(); area0 = crossProduct.lengthSquare();
} }
if (indexMaxPenetration != 1) { if (indexMaxPenetration != 1) {
// Compute the area // Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1(); mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2); Vector3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.lengthSquare(); area1 = crossProduct.lengthSquare();
} }
if (indexMaxPenetration != 2) { if (indexMaxPenetration != 2) {
// Compute the area // Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1(); mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2); Vector3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.lengthSquare(); area2 = crossProduct.lengthSquare();
} }
if (indexMaxPenetration != 3) { if (indexMaxPenetration != 3) {
// Compute the area // Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1(); mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2); Vector3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare(); area3 = crossProduct.lengthSquare();
} }
// Return the index of the contact to remove // Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3); return getMaxArea(area0, area1, area2, area3);
} }
// Return the index of maximum area // Return the index of maximum area
int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { int32_t ContactManifold::getMaxArea(float area0, float area1, float area2, float area3) const {
if (area0 < area1) { if (area0 < area1) {
if (area1 < area2) { if (area1 < area2) {
if (area2 < area3) return 3; if (area2 < area3) return 3;
else return 2; else return 2;
} }
else { else {
if (area1 < area3) return 3; if (area1 < area3) return 3;
else return 1; else return 1;
} }
} }
else { else {
if (area0 < area2) { if (area0 < area2) {
if (area2 < area3) return 3; if (area2 < area3) return 3;
else return 2; else return 2;
} }
else { else {
if (area0 < area3) return 3; if (area0 < area3) return 3;
else return 0; else return 0;
} }
} }
} }
// Clear the contact manifold // Clear the contact manifold
void ContactManifold::clear() { void ContactManifold::clear() {
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
// Call the destructor explicitly and tell the memory allocator that // Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free // the corresponding memory block is now free
mContactPoints[i]->~ContactPoint(); mContactPoints[i]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint)); mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint));
} }
mNbContactPoints = 0; mNbContactPoints = 0;
} }

351
ephysics/collision/ContactManifold.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_H
// Libraries // Libraries
#include <vector> #include <vector>
@ -37,7 +16,7 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold const uint32_t MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class declarations // Class declarations
class ContactManifold; class ContactManifold;
@ -48,24 +27,24 @@ class ContactManifold;
*/ */
struct ContactManifoldListElement { struct ContactManifoldListElement {
public: public:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the actual contact manifold /// Pointer to the actual contact manifold
ContactManifold* contactManifold; ContactManifold* contactManifold;
/// Next element of the list /// Next element of the list
ContactManifoldListElement* next; ContactManifoldListElement* next;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactManifoldListElement(ContactManifold* initContactManifold, ContactManifoldListElement(ContactManifold* initContactManifold,
ContactManifoldListElement* initNext) ContactManifoldListElement* initNext)
:contactManifold(initContactManifold), next(initNext) { :contactManifold(initContactManifold), next(initNext) {
} }
}; };
// Class ContactManifold // Class ContactManifold
@ -85,280 +64,280 @@ struct ContactManifoldListElement {
*/ */
class ContactManifold { class ContactManifold {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the first proxy shape of the contact /// Pointer to the first proxy shape of the contact
ProxyShape* mShape1; ProxyShape* mShape1;
/// Pointer to the second proxy shape of the contact /// Pointer to the second proxy shape of the contact
ProxyShape* mShape2; ProxyShape* mShape2;
/// Contact points in the manifold /// Contact points in the manifold
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Normal direction Id (Unique Id representing the normal direction) /// Normal direction Id (Unique Id representing the normal direction)
short int mNormalDirectionId; int16_t mNormalDirectionId;
/// Number of contacts in the cache /// Number of contacts in the cache
uint mNbContactPoints; uint32_t mNbContactPoints;
/// First friction vector of the contact manifold /// First friction vector of the contact manifold
Vector3 mFrictionVector1; Vector3 mFrictionVector1;
/// Second friction vector of the contact manifold /// Second friction vector of the contact manifold
Vector3 mFrictionVector2; Vector3 mFrictionVector2;
/// First friction constraint accumulated impulse /// First friction constraint accumulated impulse
decimal mFrictionImpulse1; float mFrictionImpulse1;
/// Second friction constraint accumulated impulse /// Second friction constraint accumulated impulse
decimal mFrictionImpulse2; float mFrictionImpulse2;
/// Twist friction constraint accumulated impulse /// Twist friction constraint accumulated impulse
decimal mFrictionTwistImpulse; float mFrictionTwistImpulse;
/// Accumulated rolling resistance impulse /// Accumulated rolling resistance impulse
Vector3 mRollingResistanceImpulse; Vector3 mRollingResistanceImpulse;
/// True if the contact manifold has already been added into an island /// True if the contact manifold has already been added int32_to an island
bool mIsAlreadyInIsland; bool mIsAlreadyInIsland;
/// Reference to the memory allocator /// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator; MemoryAllocator& mMemoryAllocator;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ContactManifold(const ContactManifold& contactManifold); ContactManifold(const ContactManifold& contactManifold);
/// Private assignment operator /// Private assignment operator
ContactManifold& operator=(const ContactManifold& contactManifold); ContactManifold& operator=(const ContactManifold& contactManifold);
/// Return the index of maximum area /// Return the index of maximum area
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; int32_t getMaxArea(float area0, float area1, float area2, float area3) const;
/// Return the index of the contact with the larger penetration depth. /// Return the index of the contact with the larger penetration depth.
int getIndexOfDeepestPenetration(ContactPoint* newContact) const; int32_t getIndexOfDeepestPenetration(ContactPoint* newContact) const;
/// Return the index that will be removed. /// Return the index that will be removed.
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; int32_t getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const;
/// Remove a contact point from the manifold /// Remove a contact point from the manifold
void removeContactPoint(uint index); void removeContactPoint(uint32_t index);
/// Return true if the contact manifold has already been added into an island /// Return true if the contact manifold has already been added int32_to an island
bool isAlreadyInIsland() const; bool isAlreadyInIsland() const;
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2, ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short int normalDirectionId); MemoryAllocator& memoryAllocator, int16_t normalDirectionId);
/// Destructor /// Destructor
~ContactManifold(); ~ContactManifold();
/// Return a pointer to the first proxy shape of the contact /// Return a pointer to the first proxy shape of the contact
ProxyShape* getShape1() const; ProxyShape* getShape1() const;
/// Return a pointer to the second proxy shape of the contact /// Return a pointer to the second proxy shape of the contact
ProxyShape* getShape2() const; ProxyShape* getShape2() const;
/// Return a pointer to the first body of the contact manifold /// Return a pointer to the first body of the contact manifold
CollisionBody* getBody1() const; CollisionBody* getBody1() const;
/// Return a pointer to the second body of the contact manifold /// Return a pointer to the second body of the contact manifold
CollisionBody* getBody2() const; CollisionBody* getBody2() const;
/// Return the normal direction Id /// Return the normal direction Id
short int getNormalDirectionId() const; int16_t getNormalDirectionId() const;
/// Add a contact point to the manifold /// Add a contact point to the manifold
void addContactPoint(ContactPoint* contact); void addContactPoint(ContactPoint* contact);
/// Update the contact manifold. /// Update the contact manifold.
void update(const Transform& transform1, const Transform& transform2); void update(const Transform& transform1, const Transform& transform2);
/// Clear the contact manifold /// Clear the contact manifold
void clear(); void clear();
/// Return the number of contact points in the manifold /// Return the number of contact points in the manifold
uint getNbContactPoints() const; uint32_t getNbContactPoints() const;
/// Return the first friction vector at the center of the contact manifold /// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const; const Vector3& getFrictionVector1() const;
/// set the first friction vector at the center of the contact manifold /// set the first friction vector at the center of the contact manifold
void setFrictionVector1(const Vector3& mFrictionVector1); void setFrictionVector1(const Vector3& mFrictionVector1);
/// Return the second friction vector at the center of the contact manifold /// Return the second friction vector at the center of the contact manifold
const Vector3& getFrictionVector2() const; const Vector3& getFrictionVector2() const;
/// set the second friction vector at the center of the contact manifold /// set the second friction vector at the center of the contact manifold
void setFrictionVector2(const Vector3& mFrictionVector2); void setFrictionVector2(const Vector3& mFrictionVector2);
/// Return the first friction accumulated impulse /// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const; float getFrictionImpulse1() const;
/// Set the first friction accumulated impulse /// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1); void setFrictionImpulse1(float frictionImpulse1);
/// Return the second friction accumulated impulse /// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const; float getFrictionImpulse2() const;
/// Set the second friction accumulated impulse /// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2); void setFrictionImpulse2(float frictionImpulse2);
/// Return the friction twist accumulated impulse /// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const; float getFrictionTwistImpulse() const;
/// Set the friction twist accumulated impulse /// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse); void setFrictionTwistImpulse(float frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse /// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Return a contact point of the manifold /// Return a contact point of the manifold
ContactPoint* getContactPoint(uint index) const; ContactPoint* getContactPoint(uint32_t index) const;
/// Return the normalized averaged normal vector /// Return the normalized averaged normal vector
Vector3 getAverageContactNormal() const; Vector3 getAverageContactNormal() const;
/// Return the largest depth of all the contact points /// Return the largest depth of all the contact points
decimal getLargestContactDepth() const; float getLargestContactDepth() const;
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;
friend class Island; friend class Island;
friend class CollisionBody; friend class CollisionBody;
}; };
// Return a pointer to the first proxy shape of the contact // Return a pointer to the first proxy shape of the contact
inline ProxyShape* ContactManifold::getShape1() const { inline ProxyShape* ContactManifold::getShape1() const {
return mShape1; return mShape1;
} }
// Return a pointer to the second proxy shape of the contact // Return a pointer to the second proxy shape of the contact
inline ProxyShape* ContactManifold::getShape2() const { inline ProxyShape* ContactManifold::getShape2() const {
return mShape2; return mShape2;
} }
// Return a pointer to the first body of the contact manifold // Return a pointer to the first body of the contact manifold
inline CollisionBody* ContactManifold::getBody1() const { inline CollisionBody* ContactManifold::getBody1() const {
return mShape1->getBody(); return mShape1->getBody();
} }
// Return a pointer to the second body of the contact manifold // Return a pointer to the second body of the contact manifold
inline CollisionBody* ContactManifold::getBody2() const { inline CollisionBody* ContactManifold::getBody2() const {
return mShape2->getBody(); return mShape2->getBody();
} }
// Return the normal direction Id // Return the normal direction Id
inline short int ContactManifold::getNormalDirectionId() const { inline int16_t ContactManifold::getNormalDirectionId() const {
return mNormalDirectionId; return mNormalDirectionId;
} }
// Return the number of contact points in the manifold // Return the number of contact points in the manifold
inline uint ContactManifold::getNbContactPoints() const { inline uint32_t ContactManifold::getNbContactPoints() const {
return mNbContactPoints; return mNbContactPoints;
} }
// Return the first friction vector at the center of the contact manifold // Return the first friction vector at the center of the contact manifold
inline const Vector3& ContactManifold::getFrictionVector1() const { inline const Vector3& ContactManifold::getFrictionVector1() const {
return mFrictionVector1; return mFrictionVector1;
} }
// set the first friction vector at the center of the contact manifold // set the first friction vector at the center of the contact manifold
inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) { inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) {
mFrictionVector1 = frictionVector1; mFrictionVector1 = frictionVector1;
} }
// Return the second friction vector at the center of the contact manifold // Return the second friction vector at the center of the contact manifold
inline const Vector3& ContactManifold::getFrictionVector2() const { inline const Vector3& ContactManifold::getFrictionVector2() const {
return mFrictionVector2; return mFrictionVector2;
} }
// set the second friction vector at the center of the contact manifold // set the second friction vector at the center of the contact manifold
inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) { inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) {
mFrictionVector2 = frictionVector2; mFrictionVector2 = frictionVector2;
} }
// Return the first friction accumulated impulse // Return the first friction accumulated impulse
inline decimal ContactManifold::getFrictionImpulse1() const { inline float ContactManifold::getFrictionImpulse1() const {
return mFrictionImpulse1; return mFrictionImpulse1;
} }
// Set the first friction accumulated impulse // Set the first friction accumulated impulse
inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) { inline void ContactManifold::setFrictionImpulse1(float frictionImpulse1) {
mFrictionImpulse1 = frictionImpulse1; mFrictionImpulse1 = frictionImpulse1;
} }
// Return the second friction accumulated impulse // Return the second friction accumulated impulse
inline decimal ContactManifold::getFrictionImpulse2() const { inline float ContactManifold::getFrictionImpulse2() const {
return mFrictionImpulse2; return mFrictionImpulse2;
} }
// Set the second friction accumulated impulse // Set the second friction accumulated impulse
inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) { inline void ContactManifold::setFrictionImpulse2(float frictionImpulse2) {
mFrictionImpulse2 = frictionImpulse2; mFrictionImpulse2 = frictionImpulse2;
} }
// Return the friction twist accumulated impulse // Return the friction twist accumulated impulse
inline decimal ContactManifold::getFrictionTwistImpulse() const { inline float ContactManifold::getFrictionTwistImpulse() const {
return mFrictionTwistImpulse; return mFrictionTwistImpulse;
} }
// Set the friction twist accumulated impulse // Set the friction twist accumulated impulse
inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) { inline void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) {
mFrictionTwistImpulse = frictionTwistImpulse; mFrictionTwistImpulse = frictionTwistImpulse;
} }
// Set the accumulated rolling resistance impulse // Set the accumulated rolling resistance impulse
inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse) { inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse) {
mRollingResistanceImpulse = rollingResistanceImpulse; mRollingResistanceImpulse = rollingResistanceImpulse;
} }
// Return a contact point of the manifold // Return a contact point of the manifold
inline ContactPoint* ContactManifold::getContactPoint(uint index) const { inline ContactPoint* ContactManifold::getContactPoint(uint32_t index) const {
assert(index < mNbContactPoints); assert(index < mNbContactPoints);
return mContactPoints[index]; return mContactPoints[index];
} }
// Return true if the contact manifold has already been added into an island // Return true if the contact manifold has already been added int32_to an island
inline bool ContactManifold::isAlreadyInIsland() const { inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland; return mIsAlreadyInIsland;
} }
// Return the normalized averaged normal vector // Return the normalized averaged normal vector
inline Vector3 ContactManifold::getAverageContactNormal() const { inline Vector3 ContactManifold::getAverageContactNormal() const {
Vector3 averageNormal; Vector3 averageNormal;
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
averageNormal += mContactPoints[i]->getNormal(); averageNormal += mContactPoints[i]->getNormal();
} }
return averageNormal.getUnit(); return averageNormal.getUnit();
} }
// Return the largest depth of all the contact points // Return the largest depth of all the contact points
inline decimal ContactManifold::getLargestContactDepth() const { inline float ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f; float largestDepth = 0.0f;
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
decimal depth = mContactPoints[i]->getPenetrationDepth(); float depth = mContactPoints[i]->getPenetrationDepth();
if (depth > largestDepth) { if (depth > largestDepth) {
largestDepth = depth; largestDepth = depth;
} }
} }
return largestDepth; return largestDepth;
} }
} }
#endif

307
ephysics/collision/ContactManifoldSet.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/ContactManifoldSet.h> #include <ephysics/collision/ContactManifoldSet.h>
@ -30,207 +11,207 @@ using namespace reactphysics3d;
// Constructor // Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds) MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds)
: mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator) { mShape2(shape2), mMemoryAllocator(memoryAllocator) {
assert(nbMaxManifolds >= 1); assert(nbMaxManifolds >= 1);
} }
// Destructor // Destructor
ContactManifoldSet::~ContactManifoldSet() { ContactManifoldSet::~ContactManifoldSet() {
// Clear all the contact manifolds // Clear all the contact manifolds
clear(); clear();
} }
// Add a contact point to the manifold set // Add a contact point to the manifold set
void ContactManifoldSet::addContactPoint(ContactPoint* contact) { void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Compute an Id corresponding to the normal direction (using a cubemap) // Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contact->getNormal()); int16_t normalDirectionId = computeCubemapNormalId(contact->getNormal());
// If there is no contact manifold yet // If there is no contact manifold yet
if (mNbManifolds == 0) { if (mNbManifolds == 0) {
createManifold(normalDirectionId); createManifold(normalDirectionId);
mManifolds[0]->addContactPoint(contact); mManifolds[0]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0); assert(mManifolds[i]->getNbContactPoints() > 0);
} }
return; return;
} }
// Select the manifold with the most similar normal (if exists) // Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = 0; int32_t similarManifoldIndex = 0;
if (mNbMaxManifolds > 1) { if (mNbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
} }
// If a similar manifold has been found // If a similar manifold has been found
if (similarManifoldIndex != -1) { if (similarManifoldIndex != -1) {
// Add the contact point to that similar manifold // Add the contact point to that similar manifold
mManifolds[similarManifoldIndex]->addContactPoint(contact); mManifolds[similarManifoldIndex]->addContactPoint(contact);
assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0); assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0);
return; return;
} }
// If the maximum number of manifold has not been reached yet // If the maximum number of manifold has not been reached yet
if (mNbManifolds < mNbMaxManifolds) { if (mNbManifolds < mNbMaxManifolds) {
// Create a new manifold for the contact point // Create a new manifold for the contact point
createManifold(normalDirectionId); createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact); mManifolds[mNbManifolds-1]->addContactPoint(contact);
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0); assert(mManifolds[i]->getNbContactPoints() > 0);
} }
return; return;
} }
// The contact point will be in a new contact manifold, we now have too much // 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 // manifolds condidates. We need to remove one. We choose to keep the manifolds
// with the largest contact depth among their points // with the largest contact depth among their points
int smallestDepthIndex = -1; int32_t smallestDepthIndex = -1;
decimal minDepth = contact->getPenetrationDepth(); float minDepth = contact->getPenetrationDepth();
assert(mNbManifolds == mNbMaxManifolds); assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth(); float depth = mManifolds[i]->getLargestContactDepth();
if (depth < minDepth) { if (depth < minDepth) {
minDepth = depth; minDepth = depth;
smallestDepthIndex = i; smallestDepthIndex = i;
} }
} }
// If we do not want to keep to new manifold (not created yet) with the // If we do not want to keep to new manifold (not created yet) with the
// new contact point // new contact point
if (smallestDepthIndex == -1) { if (smallestDepthIndex == -1) {
// Delete the new contact // Delete the new contact
contact->~ContactPoint(); contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint)); mMemoryAllocator.release(contact, sizeof(ContactPoint));
return; return;
} }
assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds); assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds);
// Here we need to replace an existing manifold with a new one (that contains // Here we need to replace an existing manifold with a new one (that contains
// the new contact point) // the new contact point)
removeManifold(smallestDepthIndex); removeManifold(smallestDepthIndex);
createManifold(normalDirectionId); createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact); mManifolds[mNbManifolds-1]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0); assert(mManifolds[i]->getNbContactPoints() > 0);
} }
return; return;
} }
// Return the index of the contact manifold with a similar average normal. // Return the index of the contact manifold with a similar average normal.
// If no manifold has close enough average normal, it returns -1 // If no manifold has close enough average normal, it returns -1
int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { int32_t ContactManifoldSet::selectManifoldWithSimilarNormal(int16_t normalDirectionId) const {
// Return the Id of the manifold with the same normal direction id (if exists) // Return the Id of the manifold with the same normal direction id (if exists)
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) { if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
return i; return i;
} }
} }
return -1; return -1;
} }
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the // Each face of the cube is divided int32_to 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket // normal vector int32_to of the of the bucket and returns a unique Id for the bucket
short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const { int16_t ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
assert(normal.lengthSquare() > MACHINE_EPSILON); assert(normal.lengthSquare() > MACHINE_EPSILON);
int faceNo; int32_t faceNo;
decimal u, v; float u, v;
decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z)); float max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
Vector3 normalScaled = normal / max; Vector3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
faceNo = normalScaled.x > 0 ? 0 : 1; faceNo = normalScaled.x > 0 ? 0 : 1;
u = normalScaled.y; u = normalScaled.y;
v = normalScaled.z; v = normalScaled.z;
} }
else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) { else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
faceNo = normalScaled.y > 0 ? 2 : 3; faceNo = normalScaled.y > 0 ? 2 : 3;
u = normalScaled.x; u = normalScaled.x;
v = normalScaled.z; v = normalScaled.z;
} }
else { else {
faceNo = normalScaled.z > 0 ? 4 : 5; faceNo = normalScaled.z > 0 ? 4 : 5;
u = normalScaled.x; u = normalScaled.x;
v = normalScaled.y; v = normalScaled.y;
} }
int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); int32_t indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); int32_t indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--; if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; const int32_t nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
return faceNo * 200 + indexU * nbSubDivInFace + indexV; return faceNo * 200 + indexU * nbSubDivInFace + indexV;
} }
// Update the contact manifolds // Update the contact manifolds
void ContactManifoldSet::update() { void ContactManifoldSet::update() {
for (int i=mNbManifolds-1; i>=0; i--) { for (int32_t i=mNbManifolds-1; i>=0; i--) {
// Update the contact manifold // Update the contact manifold
mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(), mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform()); mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform());
// Remove the contact manifold if has no contact points anymore // Remove the contact manifold if has no contact points anymore
if (mManifolds[i]->getNbContactPoints() == 0) { if (mManifolds[i]->getNbContactPoints() == 0) {
removeManifold(i); removeManifold(i);
} }
} }
} }
// Clear the contact manifold set // Clear the contact manifold set
void ContactManifoldSet::clear() { void ContactManifoldSet::clear() {
// Destroy all the contact manifolds // Destroy all the contact manifolds
for (int i=mNbManifolds-1; i>=0; i--) { for (int32_t i=mNbManifolds-1; i>=0; i--) {
removeManifold(i); removeManifold(i);
} }
assert(mNbManifolds == 0); assert(mNbManifolds == 0);
} }
// Create a new contact manifold and add it to the set // Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(short int normalDirectionId) { void ContactManifoldSet::createManifold(int16_t normalDirectionId) {
assert(mNbManifolds < mNbMaxManifolds); assert(mNbManifolds < mNbMaxManifolds);
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId); ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId);
mNbManifolds++; mNbManifolds++;
} }
// Remove a contact manifold from the set // Remove a contact manifold from the set
void ContactManifoldSet::removeManifold(int index) { void ContactManifoldSet::removeManifold(int32_t index) {
assert(mNbManifolds > 0); assert(mNbManifolds > 0);
assert(index >= 0 && index < mNbManifolds); assert(index >= 0 && index < mNbManifolds);
// Delete the new contact // Delete the new contact
mManifolds[index]->~ContactManifold(); mManifolds[index]->~ContactManifold();
mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold)); mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
for (int i=index; (i+1) < mNbManifolds; i++) { for (int32_t i=index; (i+1) < mNbManifolds; i++) {
mManifolds[i] = mManifolds[i+1]; mManifolds[i] = mManifolds[i+1];
} }
mNbManifolds--; mNbManifolds--;
} }

161
ephysics/collision/ContactManifoldSet.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SET_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
// Libraries // Libraries
#include <ephysics/collision/ContactManifold.h> #include <ephysics/collision/ContactManifold.h>
@ -32,8 +11,8 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set const int32_t 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 const int32_t CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
// Class ContactManifoldSet // Class ContactManifoldSet
/** /**
@ -44,111 +23,109 @@ const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N
*/ */
class ContactManifoldSet { class ContactManifoldSet {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Maximum number of contact manifolds in the set /// Maximum number of contact manifolds in the set
int mNbMaxManifolds; int32_t mNbMaxManifolds;
/// Current number of contact manifolds in the set /// Current number of contact manifolds in the set
int mNbManifolds; int32_t mNbManifolds;
/// Pointer to the first proxy shape of the contact /// Pointer to the first proxy shape of the contact
ProxyShape* mShape1; ProxyShape* mShape1;
/// Pointer to the second proxy shape of the contact /// Pointer to the second proxy shape of the contact
ProxyShape* mShape2; ProxyShape* mShape2;
/// Reference to the memory allocator /// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator; MemoryAllocator& mMemoryAllocator;
/// Contact manifolds of the set /// Contact manifolds of the set
ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set /// Create a new contact manifold and add it to the set
void createManifold(short normalDirectionId); void createManifold(short normalDirectionId);
/// Remove a contact manifold from the set /// Remove a contact manifold from the set
void removeManifold(int index); void removeManifold(int32_t index);
// Return the index of the contact manifold with a similar average normal. // Return the index of the contact manifold with a similar average normal.
int selectManifoldWithSimilarNormal(short int normalDirectionId) const; int32_t selectManifoldWithSimilarNormal(int16_t normalDirectionId) const;
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the // Each face of the cube is divided int32_to 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket // normal vector int32_to of the of the bucket and returns a unique Id for the bucket
short int computeCubemapNormalId(const Vector3& normal) const; int16_t computeCubemapNormalId(const Vector3& normal) const;
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds); MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds);
/// Destructor /// Destructor
~ContactManifoldSet(); ~ContactManifoldSet();
/// Return the first proxy shape /// Return the first proxy shape
ProxyShape* getShape1() const; ProxyShape* getShape1() const;
/// Return the second proxy shape /// Return the second proxy shape
ProxyShape* getShape2() const; ProxyShape* getShape2() const;
/// Add a contact point to the manifold set /// Add a contact point to the manifold set
void addContactPoint(ContactPoint* contact); void addContactPoint(ContactPoint* contact);
/// Update the contact manifolds /// Update the contact manifolds
void update(); void update();
/// Clear the contact manifold set /// Clear the contact manifold set
void clear(); void clear();
/// Return the number of manifolds in the set /// Return the number of manifolds in the set
int getNbContactManifolds() const; int32_t getNbContactManifolds() const;
/// Return a given contact manifold /// Return a given contact manifold
ContactManifold* getContactManifold(int index) const; ContactManifold* getContactManifold(int32_t index) const;
/// Return the total number of contact points in the set of manifolds /// Return the total number of contact points in the set of manifolds
int getTotalNbContactPoints() const; int32_t getTotalNbContactPoints() const;
}; };
// Return the first proxy shape // Return the first proxy shape
inline ProxyShape* ContactManifoldSet::getShape1() const { inline ProxyShape* ContactManifoldSet::getShape1() const {
return mShape1; return mShape1;
} }
// Return the second proxy shape // Return the second proxy shape
inline ProxyShape* ContactManifoldSet::getShape2() const { inline ProxyShape* ContactManifoldSet::getShape2() const {
return mShape2; return mShape2;
} }
// Return the number of manifolds in the set // Return the number of manifolds in the set
inline int ContactManifoldSet::getNbContactManifolds() const { inline int32_t ContactManifoldSet::getNbContactManifolds() const {
return mNbManifolds; return mNbManifolds;
} }
// Return a given contact manifold // Return a given contact manifold
inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const { inline ContactManifold* ContactManifoldSet::getContactManifold(int32_t index) const {
assert(index >= 0 && index < mNbManifolds); assert(index >= 0 && index < mNbManifolds);
return mManifolds[index]; return mManifolds[index];
} }
// Return the total number of contact points in the set of manifolds // Return the total number of contact points in the set of manifolds
inline int ContactManifoldSet::getTotalNbContactPoints() const { inline int32_t ContactManifoldSet::getTotalNbContactPoints() const {
int nbPoints = 0; int32_t nbPoints = 0;
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
nbPoints += mManifolds[i]->getNbContactPoints(); nbPoints += mManifolds[i]->getNbContactPoints();
} }
return nbPoints; return nbPoints;
} }
} }
#endif

81
ephysics/collision/ProxyShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/ProxyShape.h> #include <ephysics/collision/ProxyShape.h>
@ -35,20 +16,20 @@ using namespace reactphysics3d;
* @param transform Transformation from collision shape local-space to body local-space * @param transform Transformation from collision shape local-space to body local-space
* @param mass Mass of the collision shape (in kilograms) * @param mass Mass of the collision shape (in kilograms)
*/ */
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass) ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, float mass)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL), mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL),
mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
} }
// Destructor // Destructor
ProxyShape::~ProxyShape() { ProxyShape::~ProxyShape() {
// Release the cached collision data memory // Release the cached collision data memory
if (mCachedCollisionData != NULL) { if (mCachedCollisionData != NULL) {
free(mCachedCollisionData); free(mCachedCollisionData);
} }
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
@ -57,36 +38,36 @@ ProxyShape::~ProxyShape() {
* @return True if the point is inside the collision shape * @return True if the point is inside the collision shape
*/ */
bool ProxyShape::testPointInside(const Vector3& worldPoint) { bool ProxyShape::testPointInside(const Vector3& worldPoint) {
const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
const Vector3 localPoint = localToWorld.getInverse() * worldPoint; const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
return mCollisionShape->testPointInside(localPoint, this); return mCollisionShape->testPointInside(localPoint, this);
} }
// Raycast method with feedback information // Raycast method with feedback information
/** /**
* @param ray Ray to use for the raycasting * @param ray Ray to use for the raycasting
* @param[out] raycastInfo Result of the raycasting that is valid only if the * @param[out] raycastInfo Result of the raycasting that is valid only if the
* methods returned true * methods returned true
* @return True if the ray hit the collision shape * @return True if the ray hit the collision shape
*/ */
bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the corresponding body is not active, it cannot be hit by rays // If the corresponding body is not active, it cannot be hit by rays
if (!mBody->isActive()) return false; if (!mBody->isActive()) return false;
// Convert the ray into the local-space of the collision shape // Convert the ray int32_to the local-space of the collision shape
const Transform localToWorldTransform = getLocalToWorldTransform(); const Transform localToWorldTransform = getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse(); const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Ray rayLocal(worldToLocalTransform * ray.point1, Ray rayLocal(worldToLocalTransform * ray.point1,
worldToLocalTransform * ray.point2, worldToLocalTransform * ray.point2,
ray.maxFraction); ray.maxFraction);
bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this); bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
// Convert the raycast info into world-space // Convert the raycast info int32_to world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal; raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal;
raycastInfo.worldNormal.normalize(); raycastInfo.worldNormal.normalize();
return isHit; return isHit;
} }

274
ephysics/collision/ProxyShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_PROXY_SHAPE_H
#define REACTPHYSICS3D_PROXY_SHAPE_H
// Libraries // Libraries
#include <ephysics/body/CollisionBody.h> #include <ephysics/body/CollisionBody.h>
@ -44,150 +23,150 @@ namespace reactphysics3d {
*/ */
class ProxyShape { class ProxyShape {
protected: protected:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the parent body /// Pointer to the parent body
CollisionBody* mBody; CollisionBody* mBody;
/// Internal collision shape /// Internal collision shape
CollisionShape* mCollisionShape; CollisionShape* mCollisionShape;
/// Local-space to parent body-space transform (does not change over time) /// Local-space to parent body-space transform (does not change over time)
Transform mLocalToBodyTransform; Transform mLocalToBodyTransform;
/// Mass (in kilogramms) of the corresponding collision shape /// Mass (in kilogramms) of the corresponding collision shape
decimal mMass; float mMass;
/// Pointer to the next proxy shape of the body (linked list) /// Pointer to the next proxy shape of the body (linked list)
ProxyShape* mNext; ProxyShape* mNext;
/// Broad-phase ID (node ID in the dynamic AABB tree) /// Broad-phase ID (node ID in the dynamic AABB tree)
int mBroadPhaseID; int32_t mBroadPhaseID;
/// Cached collision data /// Cached collision data
void* mCachedCollisionData; void* mCachedCollisionData;
/// Pointer to user data /// Pointer to user data
void* mUserData; void* mUserData;
/// Bits used to define the collision category of this shape. /// Bits used to define the collision category of this shape.
/// You can set a single bit to one to define a category value for this /// You can set a single bit to one to define a category value for this
/// shape. This value is one (0x0001) by default. This variable can be used /// shape. This value is one (0x0001) by default. This variable can be used
/// together with the mCollideWithMaskBits variable so that given /// together with the mCollideWithMaskBits variable so that given
/// categories of shapes collide with each other and do not collide with /// categories of shapes collide with each other and do not collide with
/// other categories. /// other categories.
unsigned short mCollisionCategoryBits; unsigned short mCollisionCategoryBits;
/// Bits mask used to state which collision categories this shape can /// Bits mask used to state which collision categories this shape can
/// collide with. This value is 0xFFFF by default. It means that this /// collide with. This value is 0xFFFF by default. It means that this
/// proxy shape will collide with every collision categories by default. /// proxy shape will collide with every collision categories by default.
unsigned short mCollideWithMaskBits; unsigned short mCollideWithMaskBits;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ProxyShape(const ProxyShape& proxyShape); ProxyShape(const ProxyShape& proxyShape);
/// Private assignment operator /// Private assignment operator
ProxyShape& operator=(const ProxyShape& proxyShape); ProxyShape& operator=(const ProxyShape& proxyShape);
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape, ProxyShape(CollisionBody* body, CollisionShape* shape,
const Transform& transform, decimal mass); const Transform& transform, float mass);
/// Destructor /// Destructor
virtual ~ProxyShape(); virtual ~ProxyShape();
/// Return the collision shape /// Return the collision shape
const CollisionShape* getCollisionShape() const; const CollisionShape* getCollisionShape() const;
/// Return the parent body /// Return the parent body
CollisionBody* getBody() const; CollisionBody* getBody() const;
/// Return the mass of the collision shape /// Return the mass of the collision shape
decimal getMass() const; float getMass() const;
/// Return a pointer to the user data attached to this body /// Return a pointer to the user data attached to this body
void* getUserData() const; void* getUserData() const;
/// Attach user data to this body /// Attach user data to this body
void setUserData(void* userData); void setUserData(void* userData);
/// Return the local to parent body transform /// Return the local to parent body transform
const Transform& getLocalToBodyTransform() const; const Transform& getLocalToBodyTransform() const;
/// Set the local to parent body transform /// Set the local to parent body transform
void setLocalToBodyTransform(const Transform& transform); void setLocalToBodyTransform(const Transform& transform);
/// Return the local to world transform /// Return the local to world transform
const Transform getLocalToWorldTransform() const; const Transform getLocalToWorldTransform() const;
/// Return true if a point is inside the collision shape /// Return true if a point is inside the collision shape
bool testPointInside(const Vector3& worldPoint); bool testPointInside(const Vector3& worldPoint);
/// Raycast method with feedback information /// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo); bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
/// Return the collision bits mask /// Return the collision bits mask
unsigned short getCollideWithMaskBits() const; unsigned short getCollideWithMaskBits() const;
/// Set the collision bits mask /// Set the collision bits mask
void setCollideWithMaskBits(unsigned short collideWithMaskBits); void setCollideWithMaskBits(unsigned short collideWithMaskBits);
/// Return the collision category bits /// Return the collision category bits
unsigned short getCollisionCategoryBits() const; unsigned short getCollisionCategoryBits() const;
/// Set the collision category bits /// Set the collision category bits
void setCollisionCategoryBits(unsigned short collisionCategoryBits); void setCollisionCategoryBits(unsigned short collisionCategoryBits);
/// Return the next proxy shape in the linked list of proxy shapes /// Return the next proxy shape in the linked list of proxy shapes
ProxyShape* getNext(); ProxyShape* getNext();
/// Return the next proxy shape in the linked list of proxy shapes /// Return the next proxy shape in the linked list of proxy shapes
const ProxyShape* getNext() const; const ProxyShape* getNext() const;
/// Return the pointer to the cached collision data /// Return the pointer to the cached collision data
void** getCachedCollisionData(); void** getCachedCollisionData();
/// Return the local scaling vector of the collision shape /// Return the local scaling vector of the collision shape
Vector3 getLocalScaling() const; Vector3 getLocalScaling() const;
/// Set the local scaling vector of the collision shape /// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class OverlappingPair; friend class OverlappingPair;
friend class CollisionBody; friend class CollisionBody;
friend class RigidBody; friend class RigidBody;
friend class BroadPhaseAlgorithm; friend class BroadPhaseAlgorithm;
friend class DynamicAABBTree; friend class DynamicAABBTree;
friend class CollisionDetection; friend class CollisionDetection;
friend class CollisionWorld; friend class CollisionWorld;
friend class DynamicsWorld; friend class DynamicsWorld;
friend class EPAAlgorithm; friend class EPAAlgorithm;
friend class GJKAlgorithm; friend class GJKAlgorithm;
friend class ConvexMeshShape; friend class ConvexMeshShape;
}; };
// Return the pointer to the cached collision data // Return the pointer to the cached collision data
inline void** ProxyShape::getCachedCollisionData() { inline void** ProxyShape::getCachedCollisionData() {
return &mCachedCollisionData; return &mCachedCollisionData;
} }
// Return the collision shape // Return the collision shape
/** /**
* @return Pointer to the internal collision shape * @return Pointer to the int32_ternal collision shape
*/ */
inline const CollisionShape* ProxyShape::getCollisionShape() const { inline const CollisionShape* ProxyShape::getCollisionShape() const {
return mCollisionShape; return mCollisionShape;
} }
// Return the parent body // Return the parent body
@ -195,23 +174,23 @@ inline const CollisionShape* ProxyShape::getCollisionShape() const {
* @return Pointer to the parent body * @return Pointer to the parent body
*/ */
inline CollisionBody* ProxyShape::getBody() const { inline CollisionBody* ProxyShape::getBody() const {
return mBody; return mBody;
} }
// Return the mass of the collision shape // Return the mass of the collision shape
/** /**
* @return Mass of the collision shape (in kilograms) * @return Mass of the collision shape (in kilograms)
*/ */
inline decimal ProxyShape::getMass() const { inline float ProxyShape::getMass() const {
return mMass; return mMass;
} }
// Return a pointer to the user data attached to this body // Return a pointer to the user data attached to this body
/** /**
* @return A pointer to the user data stored into the proxy shape * @return A pointer to the user data stored int32_to the proxy shape
*/ */
inline void* ProxyShape::getUserData() const { inline void* ProxyShape::getUserData() const {
return mUserData; return mUserData;
} }
// Attach user data to this body // Attach user data to this body
@ -219,36 +198,36 @@ inline void* ProxyShape::getUserData() const {
* @param userData Pointer to the user data you want to store within the proxy shape * @param userData Pointer to the user data you want to store within the proxy shape
*/ */
inline void ProxyShape::setUserData(void* userData) { inline void ProxyShape::setUserData(void* userData) {
mUserData = userData; mUserData = userData;
} }
// Return the local to parent body transform // Return the local to parent body transform
/** /**
* @return The transformation that transforms the local-space of the collision shape * @return The transformation that transforms the local-space of the collision shape
* to the local-space of the parent body * to the local-space of the parent body
*/ */
inline const Transform& ProxyShape::getLocalToBodyTransform() const { inline const Transform& ProxyShape::getLocalToBodyTransform() const {
return mLocalToBodyTransform; return mLocalToBodyTransform;
} }
// Set the local to parent body transform // Set the local to parent body transform
inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) { inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
mLocalToBodyTransform = transform; mLocalToBodyTransform = transform;
mBody->setIsSleeping(false); mBody->setIsSleeping(false);
// Notify the body that the proxy shape has to be updated in the broad-phase // Notify the body that the proxy shape has to be updated in the broad-phase
mBody->updateProxyShapeInBroadPhase(this, true); mBody->updateProxyShapeInBroadPhase(this, true);
} }
// Return the local to world transform // Return the local to world transform
/** /**
* @return The transformation that transforms the local-space of the collision * @return The transformation that transforms the local-space of the collision
* shape to the world-space * shape to the world-space
*/ */
inline const Transform ProxyShape::getLocalToWorldTransform() const { inline const Transform ProxyShape::getLocalToWorldTransform() const {
return mBody->mTransform * mLocalToBodyTransform; return mBody->mTransform * mLocalToBodyTransform;
} }
// Return the next proxy shape in the linked list of proxy shapes // Return the next proxy shape in the linked list of proxy shapes
@ -256,7 +235,7 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const {
* @return Pointer to 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
*/ */
inline ProxyShape* ProxyShape::getNext() { inline ProxyShape* ProxyShape::getNext() {
return mNext; return mNext;
} }
// Return the next proxy shape in the linked list of proxy shapes // Return the next proxy shape in the linked list of proxy shapes
@ -264,7 +243,7 @@ inline ProxyShape* ProxyShape::getNext() {
* @return Pointer to 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
*/ */
inline const ProxyShape* ProxyShape::getNext() const { inline const ProxyShape* ProxyShape::getNext() const {
return mNext; return mNext;
} }
// Return the collision category bits // Return the collision category bits
@ -272,7 +251,7 @@ inline const ProxyShape* ProxyShape::getNext() const {
* @return The collision category bits mask of the proxy shape * @return The collision category bits mask of the proxy shape
*/ */
inline unsigned short ProxyShape::getCollisionCategoryBits() const { inline unsigned short ProxyShape::getCollisionCategoryBits() const {
return mCollisionCategoryBits; return mCollisionCategoryBits;
} }
// Set the collision category bits // Set the collision category bits
@ -280,7 +259,7 @@ inline unsigned short ProxyShape::getCollisionCategoryBits() const {
* @param collisionCategoryBits The collision category bits mask of the proxy shape * @param collisionCategoryBits The collision category bits mask of the proxy shape
*/ */
inline void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) { inline void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) {
mCollisionCategoryBits = collisionCategoryBits; mCollisionCategoryBits = collisionCategoryBits;
} }
// Return the collision bits mask // Return the collision bits mask
@ -288,7 +267,7 @@ inline void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategor
* @return The bits mask that specifies with which collision category this shape will collide * @return The bits mask that specifies with which collision category this shape will collide
*/ */
inline unsigned short ProxyShape::getCollideWithMaskBits() const { inline unsigned short ProxyShape::getCollideWithMaskBits() const {
return mCollideWithMaskBits; return mCollideWithMaskBits;
} }
// Set the collision bits mask // Set the collision bits mask
@ -296,7 +275,7 @@ inline unsigned short ProxyShape::getCollideWithMaskBits() const {
* @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide * @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide
*/ */
inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) { inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
mCollideWithMaskBits = collideWithMaskBits; mCollideWithMaskBits = collideWithMaskBits;
} }
// Return the local scaling vector of the collision shape // Return the local scaling vector of the collision shape
@ -304,7 +283,7 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit
* @return The local scaling vector * @return The local scaling vector
*/ */
inline Vector3 ProxyShape::getLocalScaling() const { inline Vector3 ProxyShape::getLocalScaling() const {
return mCollisionShape->getScaling(); return mCollisionShape->getScaling();
} }
// Set the local scaling vector of the collision shape // Set the local scaling vector of the collision shape
@ -313,15 +292,14 @@ inline Vector3 ProxyShape::getLocalScaling() const {
*/ */
inline void ProxyShape::setLocalScaling(const Vector3& scaling) { inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
// Set the local scaling of the collision shape // Set the local scaling of the collision shape
mCollisionShape->setLocalScaling(scaling); mCollisionShape->setLocalScaling(scaling);
mBody->setIsSleeping(false); mBody->setIsSleeping(false);
// Notify the body that the proxy shape has to be updated in the broad-phase // Notify the body that the proxy shape has to be updated in the broad-phase
mBody->updateProxyShapeInBroadPhase(this, true); mBody->updateProxyShapeInBroadPhase(this, true);
} }
} }
#endif

53
ephysics/collision/RaycastInfo.cpp
View File

@ -1,49 +1,30 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/decimal.h>
#include <ephysics/collision/RaycastInfo.h> #include <ephysics/collision/RaycastInfo.h>
#include <ephysics/collision/ProxyShape.h> #include <ephysics/collision/ProxyShape.h>
using namespace reactphysics3d; using namespace reactphysics3d;
// Ray cast test against a proxy shape // Ray cast test against a proxy shape
decimal RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) { float RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) {
// Ray casting test against the collision shape // Ray casting test against the collision shape
RaycastInfo raycastInfo; RaycastInfo raycastInfo;
bool isHit = shape->raycast(ray, raycastInfo); bool isHit = shape->raycast(ray, raycastInfo);
// If the ray hit the collision shape // If the ray hit the collision shape
if (isHit) { if (isHit) {
// Report the hit to the user and return the // Report the hit to the user and return the
// user hit fraction value // user hit fraction value
return userCallback->notifyRaycastHit(raycastInfo); return userCallback->notifyRaycastHit(raycastInfo);
} }
return ray.maxFraction; return ray.maxFraction;
} }

154
ephysics/collision/RaycastInfo.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_RAYCAST_INFO_H
#define REACTPHYSICS3D_RAYCAST_INFO_H
// Libraries // Libraries
#include <ephysics/mathematics/Vector3.h> #include <ephysics/mathematics/Vector3.h>
@ -44,53 +23,53 @@ class CollisionShape;
*/ */
struct RaycastInfo { struct RaycastInfo {
private: private:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy constructor /// Private copy constructor
RaycastInfo(const RaycastInfo& raycastInfo); RaycastInfo(const RaycastInfo& raycastInfo);
/// Private assignment operator /// Private assignment operator
RaycastInfo& operator=(const RaycastInfo& raycastInfo); RaycastInfo& operator=(const RaycastInfo& raycastInfo);
public: public:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Hit point in world-space coordinates /// Hit point in world-space coordinates
Vector3 worldPoint; Vector3 worldPoint;
/// Surface normal at hit point in world-space coordinates /// Surface normal at hit point in world-space coordinates
Vector3 worldNormal; Vector3 worldNormal;
/// Fraction distance of the hit point between point1 and point2 of the ray /// Fraction distance of the hit point between point1 and point2 of the ray
/// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1) /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
decimal hitFraction; float hitFraction;
/// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) /// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
int meshSubpart; int32_t meshSubpart;
/// Hit triangle index (only used for triangles mesh and -1 otherwise) /// Hit triangle index (only used for triangles mesh and -1 otherwise)
int triangleIndex; int32_t triangleIndex;
/// Pointer to the hit collision body /// Pointer to the hit collision body
CollisionBody* body; CollisionBody* body;
/// Pointer to the hit proxy collision shape /// Pointer to the hit proxy collision shape
ProxyShape* proxyShape; ProxyShape* proxyShape;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) { RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) {
} }
/// Destructor /// Destructor
virtual ~RaycastInfo() { virtual ~RaycastInfo() {
} }
}; };
// Class RaycastCallback // Class RaycastCallback
@ -102,51 +81,50 @@ struct RaycastInfo {
*/ */
class RaycastCallback { class RaycastCallback {
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Destructor /// Destructor
virtual ~RaycastCallback() { virtual ~RaycastCallback() {
} }
/// This method will be called for each ProxyShape that is hit by the /// This method will be called for each ProxyShape that is hit by the
/// ray. You cannot make any assumptions about the order of the /// ray. You cannot make any assumptions about the order of the
/// calls. You should use the return value to control the continuation /// calls. You should use the return value to control the continuation
/// of the ray. The returned value is the next maxFraction value to use. /// of the ray. The returned value is the next maxFraction value to use.
/// If you return a fraction of 0.0, it means that the raycast should /// If you return a fraction of 0.0, it means that the raycast should
/// terminate. If you return a fraction of 1.0, it indicates that the /// terminate. If you return a fraction of 1.0, it indicates that the
/// ray is not clipped and the ray cast should continue as if no hit /// ray is not clipped and the ray cast should continue as if no hit
/// occurred. If you return the fraction in the parameter (hitFraction /// occurred. If you return the fraction in the parameter (hitFraction
/// value in the RaycastInfo object), the current ray will be clipped /// value in the RaycastInfo object), the current ray will be clipped
/// to this fraction in the next queries. If you return -1.0, it will /// to this fraction in the next queries. If you return -1.0, it will
/// ignore this ProxyShape and continue the ray cast. /// ignore this ProxyShape and continue the ray cast.
/** /**
* @param raycastInfo Information about the raycast hit * @param raycastInfo Information about the raycast hit
* @return Value that controls the continuation of the ray after a hit * @return Value that controls the continuation of the ray after a hit
*/ */
virtual decimal notifyRaycastHit(const RaycastInfo& raycastInfo)=0; virtual float notifyRaycastHit(const RaycastInfo& raycastInfo)=0;
}; };
/// Structure RaycastTest /// Structure RaycastTest
struct RaycastTest { struct RaycastTest {
public: public:
/// User callback class /// User callback class
RaycastCallback* userCallback; RaycastCallback* userCallback;
/// Constructor /// Constructor
RaycastTest(RaycastCallback* callback) { RaycastTest(RaycastCallback* callback) {
userCallback = callback; userCallback = callback;
} }
/// Ray cast test against a proxy shape /// Ray cast test against a proxy shape
decimal raycastAgainstShape(ProxyShape* shape, const Ray& ray); float raycastAgainstShape(ProxyShape* shape, const Ray& ray);
}; };
} }
#endif

29
ephysics/collision/TriangleMesh.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/TriangleMesh.h> #include <ephysics/collision/TriangleMesh.h>

70
ephysics/collision/TriangleMesh.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_MESH_H
#define REACTPHYSICS3D_TRIANGLE_MESH_H
// Libraries // Libraries
#include <vector> #include <vector>
@ -43,46 +22,45 @@ namespace reactphysics3d {
*/ */
class TriangleMesh { class TriangleMesh {
protected: protected:
/// All the triangle arrays of the mesh (one triangle array per part) /// All the triangle arrays of the mesh (one triangle array per part)
std::vector<TriangleVertexArray*> mTriangleArrays; std::vector<TriangleVertexArray*> mTriangleArrays;
public: public:
/// Constructor /// Constructor
TriangleMesh(); TriangleMesh();
/// Destructor /// Destructor
virtual ~TriangleMesh(); virtual ~TriangleMesh();
/// Add a subpart of the mesh /// Add a subpart of the mesh
void addSubpart(TriangleVertexArray* triangleVertexArray); void addSubpart(TriangleVertexArray* triangleVertexArray);
/// Return a pointer to a given subpart (triangle vertex array) of the mesh /// Return a pointer to a given subpart (triangle vertex array) of the mesh
TriangleVertexArray* getSubpart(uint indexSubpart) const; TriangleVertexArray* getSubpart(uint32_t indexSubpart) const;
/// Return the number of subparts of the mesh /// Return the number of subparts of the mesh
uint getNbSubparts() const; uint32_t getNbSubparts() const;
}; };
// Add a subpart of the mesh // Add a subpart of the mesh
inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) { inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) {
mTriangleArrays.push_back(triangleVertexArray ); mTriangleArrays.push_back(triangleVertexArray );
} }
// Return a pointer to a given subpart (triangle vertex array) of the mesh // Return a pointer to a given subpart (triangle vertex array) of the mesh
inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const { inline TriangleVertexArray* TriangleMesh::getSubpart(uint32_t indexSubpart) const {
assert(indexSubpart < mTriangleArrays.size()); assert(indexSubpart < mTriangleArrays.size());
return mTriangleArrays[indexSubpart]; return mTriangleArrays[indexSubpart];
} }
// Return the number of subparts of the mesh // Return the number of subparts of the mesh
inline uint TriangleMesh::getNbSubparts() const { inline uint32_t TriangleMesh::getNbSubparts() const {
return mTriangleArrays.size(); return mTriangleArrays.size();
} }
} }
#endif

55
ephysics/collision/TriangleVertexArray.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/TriangleVertexArray.h> #include <ephysics/collision/TriangleVertexArray.h>
@ -29,7 +10,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
/// Note that your data will not be copied into the TriangleVertexArray and /// Note that your data will not be copied int32_to the TriangleVertexArray and
/// therefore, you need to make sure that those data are always valid during /// therefore, you need to make sure that those data are always valid during
/// the lifetime of the TriangleVertexArray. /// the lifetime of the TriangleVertexArray.
/** /**
@ -40,19 +21,19 @@ using namespace reactphysics3d;
* @param indexesStart Pointer to the first triangle index * @param indexesStart Pointer to the first triangle index
* @param indexesStride Number of bytes between the beginning of two consecutive triangle indices * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
* @param vertexDataType Type of data for the vertices (float, double) * @param vertexDataType Type of data for the vertices (float, double)
* @param indexDataType Type of data for the indices (short, int) * @param indexDataType Type of data for the indices (short, int32_t)
*/ */
TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, TriangleVertexArray::TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride, uint32_t nbTriangles, void* indexesStart, int32_t indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) { VertexDataType vertexDataType, IndexDataType indexDataType) {
m_numberVertices = nbVertices; m_numberVertices = nbVertices;
m_verticesStart = reinterpret_cast<unsigned char*>(verticesStart); m_verticesStart = reinterpret_cast<unsigned char*>(verticesStart);
m_verticesStride = verticesStride; m_verticesStride = verticesStride;
mNbTriangles = nbTriangles; mNbTriangles = nbTriangles;
mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart); mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
mIndicesStride = indexesStride; mIndicesStride = indexesStride;
mVertexDataType = vertexDataType; mVertexDataType = vertexDataType;
mIndexDataType = indexDataType; mIndexDataType = indexDataType;
} }
// Destructor // Destructor

152
ephysics/collision/TriangleVertexArray.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_VERTEX_ARRAY_H
#define REACTPHYSICS3D_TRIANGLE_VERTEX_ARRAY_H
// Libraries // Libraries
#include <ephysics/configuration.h> #include <ephysics/configuration.h>
@ -36,87 +15,87 @@ namespace reactphysics3d {
* This class is used to describe the vertices and faces of a triangular mesh. * This class is used to describe the vertices and faces of a triangular mesh.
* A TriangleVertexArray represents a continuous array of vertices and indexes * A TriangleVertexArray represents a continuous array of vertices and indexes
* of a triangular mesh. When you create a TriangleVertexArray, no data is copied * of a triangular mesh. When you create a TriangleVertexArray, no data is copied
* into the array. It only stores pointer to the data. The purpose is to allow * int32_to 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 * the user to share vertices data between the physics engine and the rendering
* part. Therefore, make sure that the data pointed by a TriangleVertexArray * part. Therefore, make sure that the data pointed by a TriangleVertexArray
* remains valid during the TriangleVertexArray life. * remains valid during the TriangleVertexArray life.
*/ */
class TriangleVertexArray { class TriangleVertexArray {
public: public:
/// Data type for the vertices in the array /// Data type for the vertices in the array
enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
/// Data type for the indices in the array /// Data type for the indices in the array
enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
protected: protected:
/// Number of vertices in the array /// Number of vertices in the array
uint m_numberVertices; uint32_t m_numberVertices;
/// Pointer to the first vertex value in the array /// Pointer to the first vertex value in the array
unsigned char* m_verticesStart; unsigned char* m_verticesStart;
/// Stride (number of bytes) between the beginning of two vertices /// Stride (number of bytes) between the beginning of two vertices
/// values in the array /// values in the array
int m_verticesStride; int32_t m_verticesStride;
/// Number of triangles in the array /// Number of triangles in the array
uint mNbTriangles; uint32_t mNbTriangles;
/// Pointer to the first vertex index of the array /// Pointer to the first vertex index of the array
unsigned char* mIndicesStart; unsigned char* mIndicesStart;
/// Stride (number of bytes) between the beginning of two indices in /// Stride (number of bytes) between the beginning of two indices in
/// the array /// the array
int mIndicesStride; int32_t mIndicesStride;
/// Data type of the vertices in the array /// Data type of the vertices in the array
VertexDataType mVertexDataType; VertexDataType mVertexDataType;
/// Data type of the indices in the array /// Data type of the indices in the array
IndexDataType mIndexDataType; IndexDataType mIndexDataType;
public: public:
/// Constructor /// Constructor
TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride, uint32_t nbTriangles, void* indexesStart, int32_t indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType); VertexDataType vertexDataType, IndexDataType indexDataType);
/// Destructor /// Destructor
virtual ~TriangleVertexArray(); virtual ~TriangleVertexArray();
/// Return the vertex data type /// Return the vertex data type
VertexDataType getVertexDataType() const; VertexDataType getVertexDataType() const;
/// Return the index data type /// Return the index data type
IndexDataType getIndexDataType() const; IndexDataType getIndexDataType() const;
/// Return the number of vertices /// Return the number of vertices
uint getNbVertices() const; uint32_t getNbVertices() const;
/// Return the number of triangles /// Return the number of triangles
uint getNbTriangles() const; uint32_t getNbTriangles() const;
/// Return the vertices stride (number of bytes) /// Return the vertices stride (number of bytes)
int getVerticesStride() const; int32_t getVerticesStride() const;
/// Return the indices stride (number of bytes) /// Return the indices stride (number of bytes)
int getIndicesStride() const; int32_t getIndicesStride() const;
/// Return the pointer to the start of the vertices array /// Return the pointer to the start of the vertices array
unsigned char* getVerticesStart() const; unsigned char* getVerticesStart() const;
/// Return the pointer to the start of the indices array /// Return the pointer to the start of the indices array
unsigned char* getIndicesStart() const; unsigned char* getIndicesStart() const;
}; };
// Return the vertex data type // Return the vertex data type
inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const { inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const {
return mVertexDataType; return mVertexDataType;
} }
// Return the index data type // Return the index data type
@ -125,36 +104,35 @@ inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType(
} }
// Return the number of vertices // Return the number of vertices
inline uint TriangleVertexArray::getNbVertices() const { inline uint32_t TriangleVertexArray::getNbVertices() const {
return m_numberVertices; return m_numberVertices;
} }
// Return the number of triangles // Return the number of triangles
inline uint TriangleVertexArray::getNbTriangles() const { inline uint32_t TriangleVertexArray::getNbTriangles() const {
return mNbTriangles; return mNbTriangles;
} }
// Return the vertices stride (number of bytes) // Return the vertices stride (number of bytes)
inline int TriangleVertexArray::getVerticesStride() const { inline int32_t TriangleVertexArray::getVerticesStride() const {
return m_verticesStride; return m_verticesStride;
} }
// Return the indices stride (number of bytes) // Return the indices stride (number of bytes)
inline int TriangleVertexArray::getIndicesStride() const { inline int32_t TriangleVertexArray::getIndicesStride() const {
return mIndicesStride; return mIndicesStride;
} }
// Return the pointer to the start of the vertices array // Return the pointer to the start of the vertices array
inline unsigned char* TriangleVertexArray::getVerticesStart() const { inline unsigned char* TriangleVertexArray::getVerticesStart() const {
return m_verticesStart; return m_verticesStart;
} }
// Return the pointer to the start of the indices array // Return the pointer to the start of the indices array
inline unsigned char* TriangleVertexArray::getIndicesStart() const { inline unsigned char* TriangleVertexArray::getIndicesStart() const {
return mIndicesStart; return mIndicesStart;
} }
} }
#endif

369
ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/broadphase/BroadPhaseAlgorithm.h> #include <ephysics/collision/broadphase/BroadPhaseAlgorithm.h>
@ -33,260 +14,260 @@ using namespace reactphysics3d;
// Constructor // Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8), :m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8), mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
mCollisionDetection(collisionDetection) { mCollisionDetection(collisionDetection) {
// Allocate memory for the array of non-static proxy shapes IDs // Allocate memory for the array of non-static proxy shapes IDs
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(mMovedShapes != NULL);
// Allocate memory for the array of potential overlapping pairs // Allocate memory for the array of potential overlapping pairs
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs != NULL); assert(mPotentialPairs != NULL);
} }
// Destructor // Destructor
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// Release the memory for the array of non-static proxy shapes IDs // Release the memory for the array of non-static proxy shapes IDs
free(mMovedShapes); free(mMovedShapes);
// Release the memory for the array of potential overlapping pairs // Release the memory for the array of potential overlapping pairs
free(mPotentialPairs); free(mPotentialPairs);
} }
// Add a collision shape in the array of shapes that have moved in the last simulation step // 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. // and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) { void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) {
// Allocate more elements in the array of shapes that have moved if necessary // Allocate more elements in the array of shapes that have moved if necessary
if (mNbAllocatedMovedShapes == mNbMovedShapes) { if (mNbAllocatedMovedShapes == mNbMovedShapes) {
mNbAllocatedMovedShapes *= 2; mNbAllocatedMovedShapes *= 2;
int* oldArray = mMovedShapes; int32_t* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(mMovedShapes != NULL);
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int)); memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int32_t));
free(oldArray); free(oldArray);
} }
// Store the broad-phase ID into the array of shapes that have moved // Store the broad-phase ID int32_to the array of shapes that have moved
assert(mNbMovedShapes < mNbAllocatedMovedShapes); assert(mNbMovedShapes < mNbAllocatedMovedShapes);
assert(mMovedShapes != NULL); assert(mMovedShapes != NULL);
mMovedShapes[mNbMovedShapes] = broadPhaseID; mMovedShapes[mNbMovedShapes] = broadPhaseID;
mNbMovedShapes++; mNbMovedShapes++;
} }
// Remove a collision shape from the array of shapes that have moved in the last simulation step // 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. // and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t broadPhaseID) {
assert(mNbNonUsedMovedShapes <= mNbMovedShapes); assert(mNbNonUsedMovedShapes <= mNbMovedShapes);
// If less than the quarter of allocated elements of the non-static shapes IDs array // If less than the quarter of allocated elements of the non-static shapes IDs array
// are used, we release some allocated memory // are used, we release some allocated memory
if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 && if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
mNbAllocatedMovedShapes > 8) { mNbAllocatedMovedShapes > 8) {
mNbAllocatedMovedShapes /= 2; mNbAllocatedMovedShapes /= 2;
int* oldArray = mMovedShapes; int32_t* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(mMovedShapes != NULL);
uint nbElements = 0; uint32_t nbElements = 0;
for (uint i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<mNbMovedShapes; i++) {
if (oldArray[i] != -1) { if (oldArray[i] != -1) {
mMovedShapes[nbElements] = oldArray[i]; mMovedShapes[nbElements] = oldArray[i];
nbElements++; nbElements++;
} }
} }
mNbMovedShapes = nbElements; mNbMovedShapes = nbElements;
mNbNonUsedMovedShapes = 0; mNbNonUsedMovedShapes = 0;
free(oldArray); free(oldArray);
} }
// Remove the broad-phase ID from the array // Remove the broad-phase ID from the array
for (uint i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<mNbMovedShapes; i++) {
if (mMovedShapes[i] == broadPhaseID) { if (mMovedShapes[i] == broadPhaseID) {
mMovedShapes[i] = -1; mMovedShapes[i] = -1;
mNbNonUsedMovedShapes++; mNbNonUsedMovedShapes++;
break; break;
} }
} }
} }
// Add a proxy collision shape into the broad-phase collision detection // Add a proxy collision shape int32_to the broad-phase collision detection
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID // Add the collision shape int32_to the dynamic AABB tree and get its broad-phase ID
int nodeId = m_dynamicAABBTree.addObject(aabb, proxyShape); int32_t nodeId = m_dynamicAABBTree.addObject(aabb, proxyShape);
// Set the broad-phase ID of the proxy shape // Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeId; proxyShape->mBroadPhaseID = nodeId;
// Add the collision shape into the array of bodies that have moved (or have been created) // Add the collision shape int32_to the array of bodies that have moved (or have been created)
// during the last simulation step // during the last simulation step
addMovedCollisionShape(proxyShape->mBroadPhaseID); addMovedCollisionShape(proxyShape->mBroadPhaseID);
} }
// Remove a proxy collision shape from the broad-phase collision detection // Remove a proxy collision shape from the broad-phase collision detection
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
int broadPhaseID = proxyShape->mBroadPhaseID; int32_t broadPhaseID = proxyShape->mBroadPhaseID;
// Remove the collision shape from the dynamic AABB tree // Remove the collision shape from the dynamic AABB tree
m_dynamicAABBTree.removeObject(broadPhaseID); m_dynamicAABBTree.removeObject(broadPhaseID);
// Remove the collision shape into the array of shapes that have moved (or have been created) // Remove the collision shape int32_to the array of shapes that have moved (or have been created)
// during the last simulation step // during the last simulation step
removeMovedCollisionShape(broadPhaseID); removeMovedCollisionShape(broadPhaseID);
} }
// Notify the broad-phase that a collision shape has moved and need to be updated // Notify the broad-phase that a collision shape has moved and need to be updated
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert) { const Vector3& displacement, bool forceReinsert) {
int broadPhaseID = proxyShape->mBroadPhaseID; int32_t broadPhaseID = proxyShape->mBroadPhaseID;
assert(broadPhaseID >= 0); assert(broadPhaseID >= 0);
// Update the dynamic AABB tree according to the movement of the collision shape // Update the dynamic AABB tree according to the movement of the collision shape
bool hasBeenReInserted = m_dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); bool hasBeenReInserted = m_dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert);
// If the collision shape has moved out of its fat AABB (and therefore has been reinserted // If the collision shape has moved out of its fat AABB (and therefore has been reinserted
// into the tree). // int32_to the tree).
if (hasBeenReInserted) { if (hasBeenReInserted) {
// Add the collision shape into the array of shapes that have moved (or have been created) // Add the collision shape int32_to the array of shapes that have moved (or have been created)
// during the last simulation step // during the last simulation step
addMovedCollisionShape(broadPhaseID); addMovedCollisionShape(broadPhaseID);
} }
} }
// Compute all the overlapping pairs of collision shapes // Compute all the overlapping pairs of collision shapes
void BroadPhaseAlgorithm::computeOverlappingPairs() { void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Reset the potential overlapping pairs // Reset the potential overlapping pairs
mNbPotentialPairs = 0; mNbPotentialPairs = 0;
// For all collision shapes that have moved (or have been created) during the // For all collision shapes that have moved (or have been created) during the
// last simulation step // last simulation step
for (uint i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<mNbMovedShapes; i++) {
int shapeID = mMovedShapes[i]; int32_t shapeID = mMovedShapes[i];
if (shapeID == -1) continue; if (shapeID == -1) continue;
AABBOverlapCallback callback(*this, shapeID); AABBOverlapCallback callback(*this, shapeID);
// Get the AABB of the shape // Get the AABB of the shape
const AABB& shapeAABB = m_dynamicAABBTree.getFatAABB(shapeID); const AABB& shapeAABB = m_dynamicAABBTree.getFatAABB(shapeID);
// Ask the dynamic AABB tree to report all collision shapes that overlap with // Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair. // by the dynamic AABB tree for each potential overlapping pair.
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback); m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
} }
// Reset the array of collision shapes that have move (or have been created) during the // Reset the array of collision shapes that have move (or have been created) during the
// last simulation step // last simulation step
mNbMovedShapes = 0; mNbMovedShapes = 0;
// Sort the array of potential overlapping pairs in order to remove duplicate pairs // Sort the array of potential overlapping pairs in order to remove duplicate pairs
std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPhasePair::smallerThan); std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPhasePair::smallerThan);
// Check all the potential overlapping pairs avoiding duplicates to report unique // Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs // overlapping pairs
uint i=0; uint32_t i=0;
while (i < mNbPotentialPairs) { while (i < mNbPotentialPairs) {
// Get a potential overlapping pair // Get a potential overlapping pair
BroadPhasePair* pair = mPotentialPairs + i; BroadPhasePair* pair = mPotentialPairs + i;
i++; i++;
assert(pair->collisionShape1ID != pair->collisionShape2ID); assert(pair->collisionShape1ID != pair->collisionShape2ID);
// Get the two collision shapes of the pair // Get the two collision shapes of the pair
ProxyShape* shape1 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID)); ProxyShape* shape1 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID)); ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
// Notify the collision detection about the overlapping pair // Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
// Skip the duplicate overlapping pairs // Skip the duplicate overlapping pairs
while (i < mNbPotentialPairs) { while (i < mNbPotentialPairs) {
// Get the next pair // Get the next pair
BroadPhasePair* nextPair = mPotentialPairs + i; BroadPhasePair* nextPair = mPotentialPairs + i;
// If the next pair is different from the previous one, we stop skipping pairs // If the next pair is different from the previous one, we stop skipping pairs
if (nextPair->collisionShape1ID != pair->collisionShape1ID || if (nextPair->collisionShape1ID != pair->collisionShape1ID ||
nextPair->collisionShape2ID != pair->collisionShape2ID) { nextPair->collisionShape2ID != pair->collisionShape2ID) {
break; break;
} }
i++; i++;
} }
} }
// If the number of potential overlapping pairs is less than the quarter of allocated // If the number of potential overlapping pairs is less than the quarter of allocated
// number of overlapping pairs // number of overlapping pairs
if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) { if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) {
// Reduce the number of allocated potential overlapping pairs // Reduce the number of allocated potential overlapping pairs
BroadPhasePair* oldPairs = mPotentialPairs; BroadPhasePair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs /= 2; mNbAllocatedPotentialPairs /= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs); assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs); free(oldPairs);
} }
} }
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree // Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) { void BroadPhaseAlgorithm::notifyOverlappingNodes(int32_t node1ID, int32_t node2ID) {
// If both the nodes are the same, we do not create store the overlapping pair // If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return; if (node1ID == node2ID) return;
// If we need to allocate more memory for the array of potential overlapping pairs // If we need to allocate more memory for the array of potential overlapping pairs
if (mNbPotentialPairs == mNbAllocatedPotentialPairs) { if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
// Allocate more memory for the array of potential pairs // Allocate more memory for the array of potential pairs
BroadPhasePair* oldPairs = mPotentialPairs; BroadPhasePair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs *= 2; mNbAllocatedPotentialPairs *= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs); assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs); free(oldPairs);
} }
// Add the new potential pair into the array of potential overlapping pairs // Add the new potential pair int32_to the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID); mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID); mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++; mNbPotentialPairs++;
} }
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
void AABBOverlapCallback::notifyOverlappingNode(int nodeId) { void AABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) {
mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId); mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
} }
// Called for a broad-phase shape that has to be tested for raycast // Called for a broad-phase shape that has to be tested for raycast
decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { float BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) {
decimal hitFraction = decimal(-1.0); float hitFraction = float(-1.0);
// Get the proxy shape from the node // Get the proxy shape from the node
ProxyShape* proxyShape = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(nodeId)); ProxyShape* proxyShape = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(nodeId));
// Check if the raycast filtering mask allows raycast against this shape // Check if the raycast filtering mask allows raycast against this shape
if ((m_raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) { if ((m_raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
// Ask the collision detection to perform a ray cast test against // Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping // the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase // with the shape in the broad-phase
hitFraction = m_raycastTest.raycastAgainstShape(proxyShape, ray); hitFraction = m_raycastTest.raycastAgainstShape(proxyShape, ray);
} }
return hitFraction; return hitFraction;
} }

249
ephysics/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_BROAD_PHASE_ALGORITHM_H
#define REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H
// Libraries // Libraries
#include <vector> #include <vector>
@ -47,40 +26,40 @@ class BroadPhaseAlgorithm;
*/ */
struct BroadPhasePair { struct BroadPhasePair {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Broad-phase ID of the first collision shape /// Broad-phase ID of the first collision shape
int collisionShape1ID; int32_t collisionShape1ID;
/// Broad-phase ID of the second collision shape /// Broad-phase ID of the second collision shape
int collisionShape2ID; int32_t collisionShape2ID;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Method used to compare two pairs for sorting algorithm /// Method used to compare two pairs for sorting algorithm
static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2); static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2);
}; };
// class AABBOverlapCallback // class AABBOverlapCallback
class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback { class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private: private:
BroadPhaseAlgorithm& mBroadPhaseAlgorithm; BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int mReferenceNodeId; int32_t mReferenceNodeId;
public: public:
// Constructor // Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId) AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int32_t referenceNodeId)
: mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) { : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
} }
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId); virtual void notifyOverlappingNode(int32_t nodeId);
}; };
@ -91,26 +70,26 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
*/ */
class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback { class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
private : private :
const DynamicAABBTree& m_dynamicAABBTree; const DynamicAABBTree& m_dynamicAABBTree;
unsigned short m_raycastWithCategoryMaskBits; unsigned short m_raycastWithCategoryMaskBits;
RaycastTest& m_raycastTest; RaycastTest& m_raycastTest;
public: public:
// Constructor // Constructor
BroadPhaseRaycastCallback(const DynamicAABBTree& dynamicAABBTree, unsigned short raycastWithCategoryMaskBits, BroadPhaseRaycastCallback(const DynamicAABBTree& dynamicAABBTree, unsigned short raycastWithCategoryMaskBits,
RaycastTest& raycastTest) RaycastTest& raycastTest)
: m_dynamicAABBTree(dynamicAABBTree), m_raycastWithCategoryMaskBits(raycastWithCategoryMaskBits), : m_dynamicAABBTree(dynamicAABBTree), m_raycastWithCategoryMaskBits(raycastWithCategoryMaskBits),
m_raycastTest(raycastTest) { m_raycastTest(raycastTest) {
} }
// Called for a broad-phase shape that has to be tested for raycast // Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray); virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray);
}; };
@ -124,125 +103,125 @@ class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
*/ */
class BroadPhaseAlgorithm { class BroadPhaseAlgorithm {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Dynamic AABB tree /// Dynamic AABB tree
DynamicAABBTree m_dynamicAABBTree; DynamicAABBTree m_dynamicAABBTree;
/// Array with the broad-phase IDs of all collision shapes that have moved (or have been /// Array with the broad-phase IDs of all collision shapes that have moved (or have been
/// created) during the last simulation step. Those are the shapes that need to be tested /// created) during the last simulation step. Those are the shapes that need to be tested
/// for overlapping in the next simulation step. /// for overlapping in the next simulation step.
int* mMovedShapes; int32_t* mMovedShapes;
/// Number of collision shapes in the array of shapes that have moved during the last /// Number of collision shapes in the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint mNbMovedShapes; uint32_t mNbMovedShapes;
/// Number of allocated elements for the array of shapes that have moved during the last /// Number of allocated elements for the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint mNbAllocatedMovedShapes; uint32_t mNbAllocatedMovedShapes;
/// Number of non-used elements in the array of shapes that have moved during the last /// Number of non-used elements in the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint mNbNonUsedMovedShapes; uint32_t mNbNonUsedMovedShapes;
/// Temporary array of potential overlapping pairs (with potential duplicates) /// Temporary array of potential overlapping pairs (with potential duplicates)
BroadPhasePair* mPotentialPairs; BroadPhasePair* mPotentialPairs;
/// Number of potential overlapping pairs /// Number of potential overlapping pairs
uint mNbPotentialPairs; uint32_t mNbPotentialPairs;
/// Number of allocated elements for the array of potential overlapping pairs /// Number of allocated elements for the array of potential overlapping pairs
uint mNbAllocatedPotentialPairs; uint32_t mNbAllocatedPotentialPairs;
/// Reference to the collision detection object /// Reference to the collision detection object
CollisionDetection& mCollisionDetection; CollisionDetection& mCollisionDetection;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm); BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm);
/// Private assignment operator /// Private assignment operator
BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm); BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
BroadPhaseAlgorithm(CollisionDetection& collisionDetection); BroadPhaseAlgorithm(CollisionDetection& collisionDetection);
/// Destructor /// Destructor
virtual ~BroadPhaseAlgorithm(); virtual ~BroadPhaseAlgorithm();
/// Add a proxy collision shape into the broad-phase collision detection /// Add a proxy collision shape int32_to the broad-phase collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
/// Remove a proxy collision shape from the broad-phase collision detection /// Remove a proxy collision shape from the broad-phase collision detection
void removeProxyCollisionShape(ProxyShape* proxyShape); void removeProxyCollisionShape(ProxyShape* proxyShape);
/// Notify the broad-phase that a collision shape has moved and need to be updated /// Notify the broad-phase that a collision shape has moved and need to be updated
void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert = false); const Vector3& displacement, bool forceReinsert = false);
/// Add a collision shape in the array of shapes that have moved in the last simulation step /// 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. /// and that need to be tested again for broad-phase overlapping.
void addMovedCollisionShape(int broadPhaseID); void addMovedCollisionShape(int32_t broadPhaseID);
/// Remove a collision shape from the array of shapes that have moved in the last simulation /// 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. /// step and that need to be tested again for broad-phase overlapping.
void removeMovedCollisionShape(int broadPhaseID); void removeMovedCollisionShape(int32_t broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2); void notifyOverlappingNodes(int32_t broadPhaseId1, int32_t broadPhaseId2);
/// Compute all the overlapping pairs of collision shapes /// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs(); void computeOverlappingPairs();
/// Return true if the two broad-phase collision shapes are overlapping /// Return true if the two broad-phase collision shapes are overlapping
bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const; bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const;
/// Ray casting method /// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest, void raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const; unsigned short raycastWithCategoryMaskBits) const;
}; };
// Method used to compare two pairs for sorting algorithm // Method used to compare two pairs for sorting algorithm
inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2) { inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2) {
if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true; if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true;
if (pair1.collisionShape1ID == pair2.collisionShape1ID) { if (pair1.collisionShape1ID == pair2.collisionShape1ID) {
return pair1.collisionShape2ID < pair2.collisionShape2ID; return pair1.collisionShape2ID < pair2.collisionShape2ID;
} }
return false; return false;
} }
// Return true if the two broad-phase collision shapes are overlapping // Return true if the two broad-phase collision shapes are overlapping
inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1, inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
const ProxyShape* shape2) const { const ProxyShape* shape2) const {
// Get the two AABBs of the collision shapes // Get the two AABBs of the collision shapes
const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(shape1->mBroadPhaseID); const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
const AABB& aabb2 = m_dynamicAABBTree.getFatAABB(shape2->mBroadPhaseID); const AABB& aabb2 = m_dynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);
// Check if the two AABBs are overlapping // Check if the two AABBs are overlapping
return aabb1.testCollision(aabb2); return aabb1.testCollision(aabb2);
} }
// Ray casting method // Ray casting method
inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const { unsigned short raycastWithCategoryMaskBits) const {
PROFILE("BroadPhaseAlgorithm::raycast()"); PROFILE("BroadPhaseAlgorithm::raycast()");
BroadPhaseRaycastCallback broadPhaseRaycastCallback(m_dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); BroadPhaseRaycastCallback broadPhaseRaycastCallback(m_dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
m_dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); m_dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
} }
} }
#endif

1117
ephysics/collision/broadphase/DynamicAABBTree.cpp
View File

File diff suppressed because it is too large Load Diff

293
ephysics/collision/broadphase/DynamicAABBTree.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_DYNAMIC_AABB_TREE_H
#define REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
// Libraries // Libraries
#include <ephysics/configuration.h> #include <ephysics/configuration.h>
@ -46,47 +25,47 @@ struct RaycastTest;
*/ */
struct TreeNode { struct TreeNode {
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
/// Null tree node constant /// Null tree node constant
const static int NULL_TREE_NODE; const static int32_t NULL_TREE_NODE;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
// A node is either in the tree (has a parent) or in the free nodes list // A node is either in the tree (has a parent) or in the free nodes list
// (has a next node) // (has a next node)
union { union {
/// Parent node ID /// Parent node ID
int32 parentID; int32_t parentID;
/// Next allocated node ID /// Next allocated node ID
int32 nextNodeID; int32_t nextNodeID;
}; };
// A node is either a leaf (has data) or is an internal node (has children) // A node is either a leaf (has data) or is an int32_ternal node (has children)
union { union {
/// Left and right child of the node (children[0] = left child) /// Left and right child of the node (children[0] = left child)
int32 children[2]; int32_t children[2];
/// Two pieces of data stored at that node (in case the node is a leaf) /// Two pieces of data stored at that node (in case the node is a leaf)
union { union {
int32 dataInt[2]; int32_t dataInt[2];
void* dataPointer; void* dataPointer;
}; };
}; };
/// Height of the node in the tree /// Height of the node in the tree
int16 height; int16_t height;
/// Fat axis aligned bounding box (AABB) corresponding to the node /// Fat axis aligned bounding box (AABB) corresponding to the node
AABB aabb; AABB aabb;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Return true if the node is a leaf of the tree /// Return true if the node is a leaf of the tree
bool isLeaf() const; bool isLeaf() const;
}; };
// Class DynamicAABBTreeOverlapCallback // Class DynamicAABBTreeOverlapCallback
@ -96,12 +75,12 @@ struct TreeNode {
*/ */
class DynamicAABBTreeOverlapCallback { class DynamicAABBTreeOverlapCallback {
public : public :
virtual ~DynamicAABBTreeOverlapCallback() = default; virtual ~DynamicAABBTreeOverlapCallback() = default;
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId)=0; virtual void notifyOverlappingNode(int32_t nodeId)=0;
}; };
// Class DynamicAABBTreeRaycastCallback // Class DynamicAABBTreeRaycastCallback
@ -111,11 +90,11 @@ class DynamicAABBTreeOverlapCallback {
*/ */
class DynamicAABBTreeRaycastCallback { class DynamicAABBTreeRaycastCallback {
public: public:
virtual ~DynamicAABBTreeRaycastCallback() = default; virtual ~DynamicAABBTreeRaycastCallback() = default;
// Called when the AABB of a leaf node is hit by a ray // Called when the AABB of a leaf node is hit by a ray
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray)=0; virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray)=0;
}; };
@ -129,166 +108,164 @@ class DynamicAABBTreeRaycastCallback {
*/ */
class DynamicAABBTree { class DynamicAABBTree {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the memory location of the nodes of the tree /// Pointer to the memory location of the nodes of the tree
TreeNode* mNodes; TreeNode* mNodes;
/// ID of the root node of the tree /// ID of the root node of the tree
int mRootNodeID; int32_t mRootNodeID;
/// ID of the first node of the list of free (allocated) nodes in the tree that we can use /// ID of the first node of the list of free (allocated) nodes in the tree that we can use
int mFreeNodeID; int32_t mFreeNodeID;
/// Number of allocated nodes in the tree /// Number of allocated nodes in the tree
int mNbAllocatedNodes; int32_t mNbAllocatedNodes;
/// Number of nodes in the tree /// Number of nodes in the tree
int mNbNodes; int32_t mNbNodes;
/// Extra AABB Gap used to allow the collision shape to move a little bit /// Extra AABB Gap used to allow the collision shape to move a little bit
/// without triggering a large modification of the tree which can be costly /// without triggering a large modification of the tree which can be costly
decimal mExtraAABBGap; float mExtraAABBGap;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree /// Allocate and return a node to use in the tree
int allocateNode(); int32_t allocateNode();
/// Release a node /// Release a node
void releaseNode(int nodeID); void releaseNode(int32_t nodeID);
/// Insert a leaf node in the tree /// Insert a leaf node in the tree
void insertLeafNode(int nodeID); void insertLeafNode(int32_t nodeID);
/// Remove a leaf node from the tree /// Remove a leaf node from the tree
void removeLeafNode(int nodeID); void removeLeafNode(int32_t nodeID);
/// Balance the sub-tree of a given node using left or right rotations. /// Balance the sub-tree of a given node using left or right rotations.
int balanceSubTreeAtNode(int nodeID); int32_t balanceSubTreeAtNode(int32_t nodeID);
/// Compute the height of a given node in the tree /// Compute the height of a given node in the tree
int computeHeight(int nodeID); int32_t computeHeight(int32_t nodeID);
/// Internally add an object into the tree /// Internally add an object int32_to the tree
int addObjectInternal(const AABB& aabb); int32_t addObjectInternal(const AABB& aabb);
/// Initialize the tree /// Initialize the tree
void init(); void init();
#ifndef NDEBUG #ifndef NDEBUG
/// Check if the tree structure is valid (for debugging purpose) /// Check if the tree structure is valid (for debugging purpose)
void check() const; void check() const;
/// Check if the node structure is valid (for debugging purpose) /// Check if the node structure is valid (for debugging purpose)
void checkNode(int nodeID) const; void checkNode(int32_t nodeID) const;
#endif #endif
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
DynamicAABBTree(decimal extraAABBGap = decimal(0.0)); DynamicAABBTree(float extraAABBGap = float(0.0));
/// Destructor /// Destructor
virtual ~DynamicAABBTree(); virtual ~DynamicAABBTree();
/// Add an object into the tree (where node data are two integers) /// Add an object int32_to the tree (where node data are two int32_tegers)
int addObject(const AABB& aabb, int32 data1, int32 data2); int32_t addObject(const AABB& aabb, int32_t data1, int32_t data2);
/// Add an object into the tree (where node data is a pointer) /// Add an object int32_to the tree (where node data is a pointer)
int addObject(const AABB& aabb, void* data); int32_t addObject(const AABB& aabb, void* data);
/// Remove an object from the tree /// Remove an object from the tree
void removeObject(int nodeID); void removeObject(int32_t nodeID);
/// Update the dynamic tree after an object has moved. /// Update the dynamic tree after an object has moved.
bool updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false); bool updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false);
/// Return the fat AABB corresponding to a given node ID /// Return the fat AABB corresponding to a given node ID
const AABB& getFatAABB(int nodeID) const; const AABB& getFatAABB(int32_t nodeID) const;
/// Return the pointer to the data array of a given leaf node of the tree /// Return the pointer to the data array of a given leaf node of the tree
int32* getNodeDataInt(int nodeID) const; int32_t* getNodeDataInt(int32_t nodeID) const;
/// Return the data pointer of a given leaf node of the tree /// Return the data pointer of a given leaf node of the tree
void* getNodeDataPointer(int nodeID) const; void* getNodeDataPointer(int32_t nodeID) const;
/// Report all shapes overlapping with the AABB given in parameter. /// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWithAABB(const AABB& aabb, void reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const; DynamicAABBTreeOverlapCallback& callback) const;
/// Ray casting method /// Ray casting method
void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const; void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const;
/// Compute the height of the tree /// Compute the height of the tree
int computeHeight(); int32_t computeHeight();
/// Return the root AABB of the tree /// Return the root AABB of the tree
AABB getRootAABB() const; AABB getRootAABB() const;
/// Clear all the nodes and reset the tree /// Clear all the nodes and reset the tree
void reset(); void reset();
}; };
// Return true if the node is a leaf of the tree // Return true if the node is a leaf of the tree
inline bool TreeNode::isLeaf() const { inline bool TreeNode::isLeaf() const {
return (height == 0); return (height == 0);
} }
// Return the fat AABB corresponding to a given node ID // Return the fat AABB corresponding to a given node ID
inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const { inline const AABB& DynamicAABBTree::getFatAABB(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
return mNodes[nodeID].aabb; return mNodes[nodeID].aabb;
} }
// Return the pointer to the data array of a given leaf node of the tree // Return the pointer to the data array of a given leaf node of the tree
inline int32* DynamicAABBTree::getNodeDataInt(int nodeID) const { inline int32_t* DynamicAABBTree::getNodeDataInt(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataInt; return mNodes[nodeID].dataInt;
} }
// Return the pointer to the data pointer of a given leaf node of the tree // Return the pointer to the data pointer of a given leaf node of the tree
inline void* DynamicAABBTree::getNodeDataPointer(int nodeID) const { inline void* DynamicAABBTree::getNodeDataPointer(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataPointer; return mNodes[nodeID].dataPointer;
} }
// Return the root AABB of the tree // Return the root AABB of the tree
inline AABB DynamicAABBTree::getRootAABB() const { inline AABB DynamicAABBTree::getRootAABB() const {
return getFatAABB(mRootNodeID); return getFatAABB(mRootNodeID);
} }
// Add an object into the tree. This method creates a new leaf node in the tree and // Add an object int32_to the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node. // returns the ID of the corresponding node.
inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) { inline int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) {
int nodeId = addObjectInternal(aabb); int32_t nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataInt[0] = data1; mNodes[nodeId].dataInt[0] = data1;
mNodes[nodeId].dataInt[1] = data2; mNodes[nodeId].dataInt[1] = data2;
return nodeId; return nodeId;
} }
// Add an object into the tree. This method creates a new leaf node in the tree and // Add an object int32_to the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node. // returns the ID of the corresponding node.
inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) { inline int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) {
int nodeId = addObjectInternal(aabb); int32_t nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataPointer = data; mNodes[nodeId].dataPointer = data;
return nodeId; return nodeId;
} }
} }
#endif

63
ephysics/collision/narrowphase/CollisionDispatch.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_DISPATCH_H
#define REACTPHYSICS3D_COLLISION_DISPATCH_H
// Libraries // Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -39,29 +18,27 @@ namespace reactphysics3d {
*/ */
class CollisionDispatch { class CollisionDispatch {
protected: protected:
public: public:
/// Constructor /// Constructor
CollisionDispatch() {} CollisionDispatch() {}
/// Destructor /// Destructor
virtual ~CollisionDispatch() {} virtual ~CollisionDispatch() {}
/// Initialize the collision dispatch configuration /// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection, virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) { MemoryAllocator* memoryAllocator) {
}
} /// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
/// Select and return the narrow-phase collision detection algorithm to virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t shape1Type,
/// use between two types of collision shapes. int32_t shape2Type)=0;
virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type,
int shape2Type)=0;
}; };
} }
#endif

391
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/ConcaveShape.h> #include <ephysics/collision/shapes/ConcaveShape.h>
@ -45,247 +26,247 @@ ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() {
// Return true and compute a contact info if the two bounding volumes collide // Return true and compute a contact info if the two bounding volumes collide
void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
ProxyShape* convexProxyShape; ProxyShape* convexProxyShape;
ProxyShape* concaveProxyShape; ProxyShape* concaveProxyShape;
const ConvexShape* convexShape; const ConvexShape* convexShape;
const ConcaveShape* concaveShape; const ConcaveShape* concaveShape;
// Collision shape 1 is convex, collision shape 2 is concave // Collision shape 1 is convex, collision shape 2 is concave
if (shape1Info.collisionShape->isConvex()) { if (shape1Info.collisionShape->isConvex()) {
convexProxyShape = shape1Info.proxyShape; convexProxyShape = shape1Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape); convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
concaveProxyShape = shape2Info.proxyShape; concaveProxyShape = shape2Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape); concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
} }
else { // Collision shape 2 is convex, collision shape 1 is concave else { // Collision shape 2 is convex, collision shape 1 is concave
convexProxyShape = shape2Info.proxyShape; convexProxyShape = shape2Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape); convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
concaveProxyShape = shape1Info.proxyShape; concaveProxyShape = shape1Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape); concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
} }
// Set the parameters of the callback object // Set the parameters of the callback object
ConvexVsTriangleCallback convexVsTriangleCallback; ConvexVsTriangleCallback convexVsTriangleCallback;
convexVsTriangleCallback.setCollisionDetection(mCollisionDetection); convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
convexVsTriangleCallback.setConvexShape(convexShape); convexVsTriangleCallback.setConvexShape(convexShape);
convexVsTriangleCallback.setConcaveShape(concaveShape); convexVsTriangleCallback.setConcaveShape(concaveShape);
convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
// Compute the convex shape AABB in the local-space of the convex shape // Compute the convex shape AABB in the local-space of the convex shape
AABB aabb; AABB aabb;
convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
// If smooth mesh collision is enabled for the concave mesh // If smooth mesh collision is enabled for the concave mesh
if (concaveShape->getIsSmoothMeshCollisionEnabled()) { if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
std::vector<SmoothMeshContactInfo> contactPoints; std::vector<SmoothMeshContactInfo> contactPoints;
SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape // Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
// Run the smooth mesh collision algorithm // Run the smooth mesh collision algorithm
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
} }
else { else {
convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape // Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
} }
} }
// Test collision between a triangle and the convex mesh shape // Test collision between a triangle and the convex mesh shape
void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape // Create a triangle collision shape
decimal margin = mConcaveShape->getTriangleMargin(); float margin = mConcaveShape->getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// Select the collision algorithm to use between the triangle and the convex shape // Select the collision algorithm to use between the triangle and the convex shape
NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(), NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
mConvexShape->getType()); mConvexShape->getType());
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (algo == NULL) return; if (algo == NULL) return;
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
algo->setCurrentOverlappingPair(mOverlappingPair); algo->setCurrentOverlappingPair(mOverlappingPair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(), CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
mOverlappingPair, mConvexProxyShape->getCachedCollisionData()); mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape, CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
mConcaveProxyShape->getLocalToWorldTransform(), mConcaveProxyShape->getLocalToWorldTransform(),
mOverlappingPair, mConcaveProxyShape->getCachedCollisionData()); mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
// Use the collision algorithm to test collision between the triangle and the other convex shape // Use the collision algorithm to test collision between the triangle and the other convex shape
algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback); algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
} }
// Process the concave triangle mesh collision using the smooth mesh collision algorithm described // 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 // by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some internal edges. // issue with some int32_ternal edges.
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
std::vector<SmoothMeshContactInfo> contactPoints, std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle // Set with the triangle vertices already processed to void further contacts with same triangle
std::unordered_multimap<int, Vector3> processTriangleVertices; std::unordered_multimap<int32_t, Vector3> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth // Sort the list of narrow-phase contacts according to their penetration depth
std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
// For each contact point (from smaller penetration depth to larger) // For each contact point (from smaller penetration depth to larger)
std::vector<SmoothMeshContactInfo>::const_iterator it; std::vector<SmoothMeshContactInfo>::const_iterator it;
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
const SmoothMeshContactInfo info = *it; const SmoothMeshContactInfo info = *it;
const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Compute the barycentric coordinates of the point in the triangle // Compute the barycentric coordinates of the point in the triangle
decimal u, v, w; float u, v, w;
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
info.triangleVertices[1], info.triangleVertices[1],
info.triangleVertices[2], info.triangleVertices[2],
contactPoint, u, v, w); contactPoint, u, v, w);
int nbZeros = 0; int32_t nbZeros = 0;
bool isUZero = approxEqual(u, 0, 0.0001); bool isUZero = approxEqual(u, 0, 0.0001);
bool isVZero = approxEqual(v, 0, 0.0001); bool isVZero = approxEqual(v, 0, 0.0001);
bool isWZero = approxEqual(w, 0, 0.0001); bool isWZero = approxEqual(w, 0, 0.0001);
if (isUZero) nbZeros++; if (isUZero) nbZeros++;
if (isVZero) nbZeros++; if (isVZero) nbZeros++;
if (isWZero) nbZeros++; if (isWZero) nbZeros++;
// If it is a vertex contact // If it is a vertex contact
if (nbZeros == 2) { if (nbZeros == 2) {
Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
// Check that this triangle vertex has not been processed yet // Check that this triangle vertex has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// Keep the contact as it is and report it // Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
} }
} }
else if (nbZeros == 1) { // If it is an edge contact else if (nbZeros == 1) { // If it is an edge contact
Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); 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]); Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
// Check that this triangle edge has not been processed yet // Check that this triangle edge has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// Keep the contact as it is and report it // Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
} }
} }
else { // If it is a face contact else { // If it is a face contact
ContactPointInfo newContactInfo(info.contactInfo); ContactPointInfo newContactInfo(info.contactInfo);
ProxyShape* firstShape; ProxyShape* firstShape;
ProxyShape* secondShape; ProxyShape* secondShape;
if (info.isFirstShapeTriangle) { if (info.isFirstShapeTriangle) {
firstShape = overlappingPair->getShape1(); firstShape = overlappingPair->getShape1();
secondShape = overlappingPair->getShape2(); secondShape = overlappingPair->getShape2();
} }
else { else {
firstShape = overlappingPair->getShape2(); firstShape = overlappingPair->getShape2();
secondShape = overlappingPair->getShape1(); secondShape = overlappingPair->getShape1();
} }
// We use the triangle normal as the contact normal // We use the triangle normal as the contact normal
Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
Vector3 localNormal = a.cross(b); Vector3 localNormal = a.cross(b);
newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
newContactInfo.normal.normalize(); newContactInfo.normal.normalize();
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
newContactInfo.normal = -newContactInfo.normal; newContactInfo.normal = -newContactInfo.normal;
} }
// We recompute the contact point on the second body with the new normal as described in // 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 // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
// Dirk Gregorius) to avoid adding torque // Dirk Gregorius) to avoid adding torque
Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
if (info.isFirstShapeTriangle) { if (info.isFirstShapeTriangle) {
Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
} }
else { else {
Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
} }
// Report the contact // Report the contact
narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
} }
// Add the three vertices of the triangle to the set of processed // Add the three vertices of the triangle to the set of processed
// triangle vertices // triangle vertices
addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
} }
} }
// Return true if the vertex is in the set of already processed vertices // 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 { bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int32_t, Vector3>& processTriangleVertices, const Vector3& vertex) const {
int key = int(vertex.x * vertex.y * vertex.z); int32_t key = int32_t(vertex.x * vertex.y * vertex.z);
auto range = processTriangleVertices.equal_range(key); auto range = processTriangleVertices.equal_range(key);
for (auto it = range.first; it != range.second; ++it) { 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; if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true;
} }
return false; return false;
} }
// Called by a narrow-phase collision algorithm when a new contact has been found // Called by a narrow-phase collision algorithm when a new contact has been found
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) { const ContactPointInfo& contactInfo) {
Vector3 triangleVertices[3]; Vector3 triangleVertices[3];
bool isFirstShapeTriangle; bool isFirstShapeTriangle;
// If the collision shape 1 is the triangle // If the collision shape 1 is the triangle
if (contactInfo.collisionShape1->getType() == TRIANGLE) { if (contactInfo.collisionShape1->getType() == TRIANGLE) {
assert(contactInfo.collisionShape2->getType() != TRIANGLE); assert(contactInfo.collisionShape2->getType() != TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1); const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
triangleVertices[0] = triangleShape->getVertex(0); triangleVertices[0] = triangleShape->getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1); triangleVertices[1] = triangleShape->getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2); triangleVertices[2] = triangleShape->getVertex(2);
isFirstShapeTriangle = true; isFirstShapeTriangle = true;
} }
else { // If the collision shape 2 is the triangle else { // If the collision shape 2 is the triangle
assert(contactInfo.collisionShape2->getType() == TRIANGLE); assert(contactInfo.collisionShape2->getType() == TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2); const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
triangleVertices[0] = triangleShape->getVertex(0); triangleVertices[0] = triangleShape->getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1); triangleVertices[1] = triangleShape->getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2); triangleVertices[2] = triangleShape->getVertex(2);
isFirstShapeTriangle = false; isFirstShapeTriangle = false;
} }
SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
// Add the narrow-phase contact into the list of contact to process for // Add the narrow-phase contact int32_to the list of contact to process for
// smooth mesh collision // smooth mesh collision
mContactPoints.push_back(smoothContactInfo); mContactPoints.push_back(smoothContactInfo);
} }

245
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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 // Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -43,68 +22,68 @@ namespace reactphysics3d {
*/ */
class ConvexVsTriangleCallback : public TriangleCallback { class ConvexVsTriangleCallback : public TriangleCallback {
protected: protected:
/// Pointer to the collision detection object /// Pointer to the collision detection object
CollisionDetection* mCollisionDetection; CollisionDetection* mCollisionDetection;
/// Narrow-phase collision callback /// Narrow-phase collision callback
NarrowPhaseCallback* mNarrowPhaseCallback; NarrowPhaseCallback* mNarrowPhaseCallback;
/// Convex collision shape to test collision with /// Convex collision shape to test collision with
const ConvexShape* mConvexShape; const ConvexShape* mConvexShape;
/// Concave collision shape /// Concave collision shape
const ConcaveShape* mConcaveShape; const ConcaveShape* mConcaveShape;
/// Proxy shape of the convex collision shape /// Proxy shape of the convex collision shape
ProxyShape* mConvexProxyShape; ProxyShape* mConvexProxyShape;
/// Proxy shape of the concave collision shape /// Proxy shape of the concave collision shape
ProxyShape* mConcaveProxyShape; ProxyShape* mConcaveProxyShape;
/// Broadphase overlapping pair /// Broadphase overlapping pair
OverlappingPair* mOverlappingPair; OverlappingPair* mOverlappingPair;
/// Used to sort ContactPointInfos according to their penetration depth /// Used to sort ContactPointInfos according to their penetration depth
static bool contactsDepthCompare(const ContactPointInfo& contact1, static bool contactsDepthCompare(const ContactPointInfo& contact1,
const ContactPointInfo& contact2); const ContactPointInfo& contact2);
public: public:
/// Set the collision detection pointer /// Set the collision detection pointer
void setCollisionDetection(CollisionDetection* collisionDetection) { void setCollisionDetection(CollisionDetection* collisionDetection) {
mCollisionDetection = collisionDetection; mCollisionDetection = collisionDetection;
} }
/// Set the narrow-phase collision callback /// Set the narrow-phase collision callback
void setNarrowPhaseCallback(NarrowPhaseCallback* callback) { void setNarrowPhaseCallback(NarrowPhaseCallback* callback) {
mNarrowPhaseCallback = callback; mNarrowPhaseCallback = callback;
} }
/// Set the convex collision shape to test collision with /// Set the convex collision shape to test collision with
void setConvexShape(const ConvexShape* convexShape) { void setConvexShape(const ConvexShape* convexShape) {
mConvexShape = convexShape; mConvexShape = convexShape;
} }
/// Set the concave collision shape /// Set the concave collision shape
void setConcaveShape(const ConcaveShape* concaveShape) { void setConcaveShape(const ConcaveShape* concaveShape) {
mConcaveShape = concaveShape; mConcaveShape = concaveShape;
} }
/// Set the broadphase overlapping pair /// Set the broadphase overlapping pair
void setOverlappingPair(OverlappingPair* overlappingPair) { void setOverlappingPair(OverlappingPair* overlappingPair) {
mOverlappingPair = overlappingPair; mOverlappingPair = overlappingPair;
} }
/// Set the proxy shapes of the two collision shapes /// Set the proxy shapes of the two collision shapes
void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) { void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) {
mConvexProxyShape = convexProxyShape; mConvexProxyShape = convexProxyShape;
mConcaveProxyShape = concaveProxyShape; mConcaveProxyShape = concaveProxyShape;
} }
/// Test collision between a triangle and the convex mesh shape /// Test collision between a triangle and the convex mesh shape
virtual void testTriangle(const Vector3* trianglePoints); virtual void testTriangle(const Vector3* trianglePoints);
}; };
// Class SmoothMeshContactInfo // Class SmoothMeshContactInfo
@ -114,35 +93,35 @@ class ConvexVsTriangleCallback : public TriangleCallback {
*/ */
class SmoothMeshContactInfo { class SmoothMeshContactInfo {
public: public:
ContactPointInfo contactInfo; ContactPointInfo contactInfo;
bool isFirstShapeTriangle; bool isFirstShapeTriangle;
Vector3 triangleVertices[3]; Vector3 triangleVertices[3];
/// Constructor /// Constructor
SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1, SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
const Vector3& trianglePoint2, const Vector3& trianglePoint3) const Vector3& trianglePoint2, const Vector3& trianglePoint3)
: contactInfo(contact) { : contactInfo(contact) {
isFirstShapeTriangle = firstShapeTriangle; isFirstShapeTriangle = firstShapeTriangle;
triangleVertices[0] = trianglePoint1; triangleVertices[0] = trianglePoint1;
triangleVertices[1] = trianglePoint2; triangleVertices[1] = trianglePoint2;
triangleVertices[2] = trianglePoint3; triangleVertices[2] = trianglePoint3;
} }
}; };
struct ContactsDepthCompare { struct ContactsDepthCompare {
bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2) bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
{ {
return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
} }
}; };
/// Method used to compare two smooth mesh contact info to sort them /// Method used to compare two smooth mesh contact info to sort them
//inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1, //inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1,
// const SmoothMeshContactInfo& contact2) { // const SmoothMeshContactInfo& contact2) {
// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; // return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
//} //}
// Class SmoothCollisionNarrowPhaseCallback // Class SmoothCollisionNarrowPhaseCallback
@ -153,23 +132,23 @@ struct ContactsDepthCompare {
*/ */
class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback { class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
private: private:
std::vector<SmoothMeshContactInfo>& mContactPoints; std::vector<SmoothMeshContactInfo>& mContactPoints;
public: public:
// Constructor // Constructor
SmoothCollisionNarrowPhaseCallback(std::vector<SmoothMeshContactInfo>& contactPoints) SmoothCollisionNarrowPhaseCallback(std::vector<SmoothMeshContactInfo>& contactPoints)
: mContactPoints(contactPoints) { : mContactPoints(contactPoints) {
} }
/// Called by a narrow-phase collision algorithm when a new contact has been found /// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo); const ContactPointInfo& contactInfo);
}; };
@ -182,53 +161,51 @@ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
*/ */
class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm); ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm);
/// Private assignment operator /// Private assignment operator
ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm); ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm);
/// Process the concave triangle mesh collision using the smooth mesh collision algorithm /// Process the concave triangle mesh collision using the smooth mesh collision algorithm
void processSmoothMeshCollision(OverlappingPair* overlappingPair, void processSmoothMeshCollision(OverlappingPair* overlappingPair,
std::vector<SmoothMeshContactInfo> contactPoints, std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
/// Add a triangle vertex into the set of processed triangles /// Add a triangle vertex int32_to the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, void addProcessedVertex(std::unordered_multimap<int32_t, Vector3>& processTriangleVertices,
const Vector3& vertex); const Vector3& vertex);
/// Return true if the vertex is in the set of already processed vertices /// Return true if the vertex is in the set of already processed vertices
bool hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, bool hasVertexBeenProcessed(const std::unordered_multimap<int32_t, Vector3>& processTriangleVertices,
const Vector3& vertex) const; const Vector3& vertex) const;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConcaveVsConvexAlgorithm(); ConcaveVsConvexAlgorithm();
/// Destructor /// Destructor
virtual ~ConcaveVsConvexAlgorithm(); virtual ~ConcaveVsConvexAlgorithm();
/// Compute a contact info if the two bounding volume collide /// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info, virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
}; };
// Add a triangle vertex into the set of processed triangles // Add a triangle vertex int32_to the set of processed triangles
inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) { inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int32_t, Vector3>& processTriangleVertices, const Vector3& vertex) {
processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex)); processTriangleVertices.insert(std::make_pair(int32_t(vertex.x * vertex.y * vertex.z), vertex));
} }
} }
#endif

76
ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/DefaultCollisionDispatch.h> #include <ephysics/collision/narrowphase/DefaultCollisionDispatch.h>
@ -41,35 +22,30 @@ DefaultCollisionDispatch::~DefaultCollisionDispatch() {
/// Initialize the collision dispatch configuration /// Initialize the collision dispatch configuration
void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection, void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) { MemoryAllocator* memoryAllocator) {
// Initialize the collision algorithms // Initialize the collision algorithms
mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator); mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
mGJKAlgorithm.init(collisionDetection, memoryAllocator); mGJKAlgorithm.init(collisionDetection, memoryAllocator);
mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator); mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator);
} }
// Select and return the narrow-phase collision detection algorithm to // Select and return the narrow-phase collision detection algorithm to
// use between two types of collision shapes. // use between two types of collision shapes.
NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) { NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int32_t type1, int32_t type2) {
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1); CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2); // Sphere vs Sphere algorithm
if (shape1Type == SPHERE && shape2Type == SPHERE) {
// Sphere vs Sphere algorithm return &mSphereVsSphereAlgorithm;
if (shape1Type == SPHERE && shape2Type == SPHERE) { } else if ( (!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))
return &mSphereVsSphereAlgorithm; || (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
} // Concave vs Convex algorithm
// Concave vs Convex algorithm return &mConcaveVsConvexAlgorithm;
else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || } else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
(!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { // Convex vs Convex algorithm (GJK algorithm)
return &mConcaveVsConvexAlgorithm; return &mGJKAlgorithm;
} } else {
// Convex vs Convex algorithm (GJK algorithm) return nullptr;
else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) { }
return &mGJKAlgorithm;
}
else {
return NULL;
}
} }

72
ephysics/collision/narrowphase/DefaultCollisionDispatch.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_DEFAULT_COLLISION_DISPATCH_H
#define REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H
// Libraries // Libraries
#include <ephysics/collision/narrowphase/CollisionDispatch.h> #include <ephysics/collision/narrowphase/CollisionDispatch.h>
@ -42,37 +21,34 @@ namespace reactphysics3d {
*/ */
class DefaultCollisionDispatch : public CollisionDispatch { class DefaultCollisionDispatch : public CollisionDispatch {
protected: protected:
/// Sphere vs Sphere collision algorithm /// Sphere vs Sphere collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
/// Concave vs Convex collision algorithm /// Concave vs Convex collision algorithm
ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm; ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
/// GJK Algorithm /// GJK Algorithm
GJKAlgorithm mGJKAlgorithm; GJKAlgorithm mGJKAlgorithm;
public: public:
/// Constructor /// Constructor
DefaultCollisionDispatch(); DefaultCollisionDispatch();
/// Destructor /// Destructor
virtual ~DefaultCollisionDispatch(); virtual ~DefaultCollisionDispatch();
/// Initialize the collision dispatch configuration /// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection, virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator); MemoryAllocator* memoryAllocator);
/// Select and return the narrow-phase collision detection algorithm to /// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes. /// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2); virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t type1, int32_t type2);
}; };
} }
#endif

665
ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.h> #include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.h>
@ -39,401 +20,401 @@ EPAAlgorithm::EPAAlgorithm() {
// Destructor // Destructor
EPAAlgorithm::~EPAAlgorithm() { EPAAlgorithm::~EPAAlgorithm() {
} }
// Decide if the origin is in the tetrahedron. // 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 /// 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 /// the vertex that is wrong if the origin is not in the tetrahedron
int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, int32_t EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const { const Vector3& p3, const Vector3& p4) const {
// Check vertex 1 // Check vertex 1
Vector3 normal1 = (p2-p1).cross(p3-p1); Vector3 normal1 = (p2-p1).cross(p3-p1);
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
return 4; return 4;
} }
// Check vertex 2 // Check vertex 2
Vector3 normal2 = (p4-p2).cross(p3-p2); Vector3 normal2 = (p4-p2).cross(p3-p2);
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
return 1; return 1;
} }
// Check vertex 3 // Check vertex 3
Vector3 normal3 = (p4-p3).cross(p1-p3); Vector3 normal3 = (p4-p3).cross(p1-p3);
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
return 2; return 2;
} }
// Check vertex 4 // Check vertex 4
Vector3 normal4 = (p2-p4).cross(p1-p4); Vector3 normal4 = (p2-p4).cross(p1-p4);
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
return 3; return 3;
} }
// The origin is in the tetrahedron, we return 0 // The origin is in the tetrahedron, we return 0
return 0; return 0;
} }
// Compute the penetration depth with the EPA algorithm. // Compute the penetration depth with the EPA algorithm.
/// This method computes the penetration depth and contact points between two /// This method computes the penetration depth and contact points between two
/// enlarged objects (with margin) where the original objects (without margin) /// enlarged objects (with margin) where the original objects (without margin)
/// intersect. An initial simplex that contains origin has been computed with /// int32_tersect. An initial simplex that contains origin has been computed with
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find /// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
/// the correct penetration depth /// the correct penetration depth
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info, CollisionShapeInfo shape1Info,
const Transform& transform1, const Transform& transform1,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
const Transform& transform2, const Transform& transform2,
Vector3& v, Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape); const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape); const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates 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 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
Vector3 points[MAX_SUPPORT_POINTS]; // Current points Vector3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face
// candidate of the EPA algorithm // candidate of the EPA algorithm
// Transform a point from local space of body 2 to local // 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) // space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2; Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local // Matrix that transform a direction from local
// space of body 1 into local space of body 2 // space of body 1 int32_to local space of body 2
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
transform1.getOrientation(); transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm // Get the simplex computed previously by the GJK algorithm
unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); uint32_t nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance // Compute the tolerance
decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); float tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope // Number of triangles in the polytope
unsigned int nbTriangles = 0; uint32_t nbTriangles = 0;
// Clear the storing of triangles // Clear the storing of triangles
triangleStore.clear(); triangleStore.clear();
// Select an action according to the number of points in the simplex // Select an action according to the number of points in the simplex
// computed with GJK algorithm in order to obtain an initial polytope for // computed with GJK algorithm in order to obtain an initial polytope for
// The EPA algorithm. // The EPA algorithm.
switch(nbVertices) { switch(nbVertices) {
case 1: case 1:
// Only one point in the simplex (which should be the origin). // Only one point in the simplex (which should be the origin).
// We have a touching contact with zero penetration depth. // We have a touching contact with zero penetration depth.
// We drop that kind of contact. Therefore, we return false // We drop that kind of contact. Therefore, we return false
return; return;
case 2: { case 2: {
// The simplex returned by GJK is a line segment d containing the origin. // 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 // 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 // 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 // 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 // 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 // construct the polytope are the three support points in those three directions
// v1, v2 and v3. // v1, v2 and v3.
// Direction of the segment // Direction of the segment
Vector3 d = (points[1] - points[0]).getUnit(); Vector3 d = (points[1] - points[0]).getUnit();
// Choose the coordinate axis from the minimal absolute component of the vector d // Choose the coordinate axis from the minimal absolute component of the vector d
int minAxis = d.getAbsoluteVector().getMinAxis(); int32_t minAxis = d.getAbsoluteVector().getMinAxis();
// Compute sin(60) // Compute sin(60)
const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5); const float sin60 = float(sqrt(3.0)) * float(0.5);
// Create a rotation quaternion to rotate the vector v1 to get the vectors // Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3 // v2 and v3
Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5);
// Compute the vector v1, v2, v3 // Compute the vector v1, v2, v3
Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
Vector3 v2 = rotationQuat * v1; Vector3 v2 = rotationQuat * v1;
Vector3 v3 = rotationQuat * v2; Vector3 v3 = rotationQuat * v2;
// Compute the support point in the direction of v1 // Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
suppPointsB[2] = body2Tobody1 * suppPointsB[2] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
points[2] = suppPointsA[2] - suppPointsB[2]; points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2 // Compute the support point in the direction of v2
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 * suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3 // Compute the support point in the direction of v3
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 * suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the // 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 // tetrahedron that contains the origin in order that the initial polytope of the
// EPA algorithm is a tetrahedron, which is simpler to deal with. // 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 the origin is in the tetrahedron of points 0, 2, 3, 4
if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) { if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) {
// We use the point 4 instead of point 1 for the initial tetrahedron // We use the point 4 instead of point 1 for the initial tetrahedron
suppPointsA[1] = suppPointsA[4]; suppPointsA[1] = suppPointsA[4];
suppPointsB[1] = suppPointsB[4]; suppPointsB[1] = suppPointsB[4];
points[1] = points[4]; points[1] = points[4];
} }
// If the origin is in the tetrahedron of points 1, 2, 3, 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) { 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 // We use the point 4 instead of point 0 for the initial tetrahedron
suppPointsA[0] = suppPointsA[4]; suppPointsA[0] = suppPointsA[4];
suppPointsB[0] = suppPointsB[4]; suppPointsB[0] = suppPointsB[4];
points[0] = points[4]; points[0] = points[4];
} }
else { else {
// The origin is not in the initial polytope // The origin is not in the initial polytope
return; return;
} }
// The polytope contains now 4 vertices // The polytope contains now 4 vertices
nbVertices = 4; nbVertices = 4;
} }
case 4: { case 4: {
// The simplex computed by the GJK algorithm is a tetrahedron. Here we check // 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 // 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 // otherwise we remove the wrong vertex of the tetrahedron and go in the case
// where the GJK algorithm compute a simplex of three vertices. // where the GJK algorithm compute a simplex of three vertices.
// Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise) // 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]); int32_t badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]);
// If the origin is in the tetrahedron // If the origin is in the tetrahedron
if (badVertex == 0) { if (badVertex == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron. // Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron // Comstruct the 4 triangle faces of the tetrahedron
TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2); TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2);
TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1); TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1);
TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3); TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
// If the constructed tetrahedron is not correct // If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return; return;
} }
// Associate the edges of neighbouring triangle faces // Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap // Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
break; break;
} }
// The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) // 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 // Remove the wrong vertex and continue to the next case with the
// three remaining vertices // three remaining vertices
if (badVertex < 4) { if (badVertex < 4) {
suppPointsA[badVertex-1] = suppPointsA[3]; suppPointsA[badVertex-1] = suppPointsA[3];
suppPointsB[badVertex-1] = suppPointsB[3]; suppPointsB[badVertex-1] = suppPointsB[3];
points[badVertex-1] = points[3]; points[badVertex-1] = points[3];
} }
// We have removed the wrong vertex // We have removed the wrong vertex
nbVertices = 3; nbVertices = 3;
} }
case 3: { case 3: {
// The GJK algorithm returned a triangle that contains the origin. // The GJK algorithm returned a triangle that contains the origin.
// We need two new vertices to create two tetrahedron. The two new // We need two new vertices to create two tetrahedron. The two new
// vertices are the support points in the "n" and "-n" direction // vertices are the support points in the "n" and "-n" direction
// where "n" is the normal of the triangle. Then, we use only the // where "n" is the normal of the triangle. Then, we use only the
// tetrahedron that contains the origin. // tetrahedron that contains the origin.
// Compute the normal of the triangle // Compute the normal of the triangle
Vector3 v1 = points[1] - points[0]; Vector3 v1 = points[1] - points[0];
Vector3 v2 = points[2] - points[0]; Vector3 v2 = points[2] - points[0];
Vector3 n = v1.cross(v2); Vector3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron // Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 * suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData); suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 * suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0 = NULL; TriangleEPA* face0 = NULL;
TriangleEPA* face1 = NULL; TriangleEPA* face1 = NULL;
TriangleEPA* face2 = NULL; TriangleEPA* face2 = NULL;
TriangleEPA* face3 = NULL; TriangleEPA* face3 = NULL;
// If the origin is in the first tetrahedron // If the origin is in the first tetrahedron
if (isOriginInTetrahedron(points[0], points[1], if (isOriginInTetrahedron(points[0], points[1],
points[2], points[3]) == 0) { points[2], points[3]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron. // Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron // Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2); face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 3, 1); face1 = triangleStore.newTriangle(points, 0, 3, 1);
face2 = triangleStore.newTriangle(points, 0, 2, 3); face2 = triangleStore.newTriangle(points, 0, 2, 3);
face3 = triangleStore.newTriangle(points, 1, 3, 2); face3 = triangleStore.newTriangle(points, 1, 3, 2);
} }
else if (isOriginInTetrahedron(points[0], points[1], else if (isOriginInTetrahedron(points[0], points[1],
points[2], points[4]) == 0) { points[2], points[4]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron. // Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron // Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2); face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 4, 1); face1 = triangleStore.newTriangle(points, 0, 4, 1);
face2 = triangleStore.newTriangle(points, 0, 2, 4); face2 = triangleStore.newTriangle(points, 0, 2, 4);
face3 = triangleStore.newTriangle(points, 1, 4, 2); face3 = triangleStore.newTriangle(points, 1, 4, 2);
} }
else { else {
return; return;
} }
// If the constructed tetrahedron is not correct // If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return; return;
} }
// Associate the edges of neighbouring triangle faces // Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap // Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
nbVertices = 4; nbVertices = 4;
} }
break; break;
} }
// At this point, we have a polytope that contains the origin. Therefore, we // At this point, we have a polytope that contains the origin. Therefore, we
// can run the EPA algorithm. // can run the EPA algorithm.
if (nbTriangles == 0) { if (nbTriangles == 0) {
return; return;
} }
TriangleEPA* triangle = 0; TriangleEPA* triangle = 0;
decimal upperBoundSquarePenDepth = DECIMAL_LARGEST; float upperBoundSquarePenDepth = DECIMAL_LARGEST;
do { do {
triangle = triangleHeap[0]; triangle = triangleHeap[0];
// Get the next candidate face (the face closest to the origin) // Get the next candidate face (the face closest to the origin)
std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison); std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison);
nbTriangles--; nbTriangles--;
// If the candidate face in the heap is not obsolete // If the candidate face in the heap is not obsolete
if (!triangle->getIsObsolete()) { if (!triangle->getIsObsolete()) {
// If we have reached the maximum number of support points // If we have reached the maximum number of support points
if (nbVertices == MAX_SUPPORT_POINTS) { if (nbVertices == MAX_SUPPORT_POINTS) {
assert(false); assert(false);
break; break;
} }
// Compute the support point of the Minkowski // Compute the support point of the Minkowski
// difference (A-B) in the closest point direction // difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(
triangle->getClosestPoint(), shape1CachedCollisionData); triangle->getClosestPoint(), shape1CachedCollisionData);
suppPointsB[nbVertices] = body2Tobody1 * suppPointsB[nbVertices] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * shape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()), shape2CachedCollisionData); (-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices; int32_t indexNewVertex = nbVertices;
nbVertices++; nbVertices++;
// Update the upper bound of the penetration depth // Update the upper bound of the penetration depth
decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
assert(wDotv > 0.0); assert(wDotv > 0.0);
decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); float wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) { if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare; upperBoundSquarePenDepth = wDotVSquare;
} }
// Compute the error // Compute the error
decimal error = wDotv - triangle->getDistSquare(); float error = wDotv - triangle->getDistSquare();
if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) || if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
points[indexNewVertex] == points[(*triangle)[0]] || points[indexNewVertex] == points[(*triangle)[0]] ||
points[indexNewVertex] == points[(*triangle)[1]] || points[indexNewVertex] == points[(*triangle)[1]] ||
points[indexNewVertex] == points[(*triangle)[2]]) { points[indexNewVertex] == points[(*triangle)[2]]) {
break; break;
} }
// Now, we compute the silhouette cast by the new vertex. The current triangle // 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 // face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face. // algorithm from the current triangle face.
int i = triangleStore.getNbTriangles(); int32_t i = triangleStore.getNbTriangles();
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
break; break;
} }
// Add all the new triangle faces computed with the silhouette algorithm // Add all the new triangle faces computed with the silhouette algorithm
// to the candidates list of faces of the current polytope // to the candidates list of faces of the current polytope
while(i != triangleStore.getNbTriangles()) { while(i != triangleStore.getNbTriangles()) {
TriangleEPA* newTriangle = &triangleStore[i]; TriangleEPA* newTriangle = &triangleStore[i];
addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth); addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
i++; i++;
} }
} }
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info // Compute the contact info
v = transform1.getOrientation() * triangle->getClosestPoint(); v = transform1.getOrientation() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit(); Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length(); float penetrationDepth = v.length();
assert(penetrationDepth > 0.0); assert(penetrationDepth > 0.0);
if (normal.lengthSquare() < MACHINE_EPSILON) return; if (normal.lengthSquare() < MACHINE_EPSILON) return;
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
} }

147
ephysics/collision/narrowphase/EPA/EPAAlgorithm.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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 // Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.h> #include <ephysics/collision/narrowphase/GJK/Simplex.h>
@ -43,10 +22,10 @@ namespace reactphysics3d {
// ---------- Constants ---------- // // ---------- Constants ---------- //
/// Maximum number of support points of the polytope /// Maximum number of support points of the polytope
const unsigned int MAX_SUPPORT_POINTS = 100; const uint32_t MAX_SUPPORT_POINTS = 100;
/// Maximum number of facets of the polytope /// Maximum number of facets of the polytope
const unsigned int MAX_FACETS = 200; const uint32_t MAX_FACETS = 200;
// Class TriangleComparison // Class TriangleComparison
@ -58,12 +37,12 @@ const unsigned int MAX_FACETS = 200;
*/ */
class TriangleComparison { class TriangleComparison {
public: public:
/// Comparison operator /// Comparison operator
bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) { bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) {
return (face1->getDistSquare() > face2->getDistSquare()); return (face1->getDistSquare() > face2->getDistSquare());
} }
}; };
@ -72,7 +51,7 @@ class TriangleComparison {
* This class is the implementation of the Expanding Polytope Algorithm (EPA). * This class is the implementation of the Expanding Polytope Algorithm (EPA).
* The EPA algorithm computes the penetration depth and contact points between * The EPA algorithm computes the penetration depth and contact points between
* two enlarged objects (with margin) where the original objects (without margin) * 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 * int32_tersect. The penetration depth of a pair of int32_tersecting objects A and B is
* the length of a point on the boundary of the Minkowski sum (A-B) closest to the * 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 * 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 * that contains the origin and expend it in order to find the point on the boundary
@ -83,79 +62,77 @@ class TriangleComparison {
*/ */
class EPAAlgorithm { class EPAAlgorithm {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Reference to the memory allocator /// Reference to the memory allocator
MemoryAllocator* mMemoryAllocator; MemoryAllocator* mMemoryAllocator;
/// Triangle comparison operator /// Triangle comparison operator
TriangleComparison mTriangleComparison; TriangleComparison mTriangleComparison;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
EPAAlgorithm(const EPAAlgorithm& algorithm); EPAAlgorithm(const EPAAlgorithm& algorithm);
/// Private assignment operator /// Private assignment operator
EPAAlgorithm& operator=(const EPAAlgorithm& algorithm); EPAAlgorithm& operator=(const EPAAlgorithm& algorithm);
/// Add a triangle face in the candidate triangle heap /// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles, void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint32_t& nbTriangles,
decimal upperBoundSquarePenDepth); float upperBoundSquarePenDepth);
/// Decide if the origin is in the tetrahedron. /// Decide if the origin is in the tetrahedron.
int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, int32_t isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const; const Vector3& p3, const Vector3& p4) const;
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
EPAAlgorithm(); EPAAlgorithm();
/// Destructor /// Destructor
~EPAAlgorithm(); ~EPAAlgorithm();
/// Initalize the algorithm /// Initalize the algorithm
void init(MemoryAllocator* memoryAllocator); void init(MemoryAllocator* memoryAllocator);
/// Compute the penetration depth with EPA algorithm. /// Compute the penetration depth with EPA algorithm.
void computePenetrationDepthAndContactPoints(const Simplex& simplex, void computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info, CollisionShapeInfo shape1Info,
const Transform& transform1, const Transform& transform1,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
const Transform& transform2, const Transform& transform2,
Vector3& v, Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
}; };
// Add a triangle face in the candidate triangle heap in the EPA algorithm // Add a triangle face in the candidate triangle heap in the EPA algorithm
inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
uint& nbTriangles, decimal upperBoundSquarePenDepth) { uint32_t& nbTriangles, float upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle // If the closest point of the affine hull of triangle
// points is internal to the triangle and if the distance // points is int32_ternal to the triangle and if the distance
// of the closest point from the origin is at most the // of the closest point from the origin is at most the
// penetration depth upper bound // penetration depth upper bound
if (triangle->isClosestPointInternalToTriangle() && if (triangle->isClosestPointInternalToTriangle() &&
triangle->getDistSquare() <= upperBoundSquarePenDepth) { triangle->getDistSquare() <= upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates // Add the triangle face to the list of candidates
heap[nbTriangles] = triangle; heap[nbTriangles] = triangle;
nbTriangles++; nbTriangles++;
std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison); std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison);
} }
} }
// Initalize the algorithm // Initalize the algorithm
inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) { inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) {
mMemoryAllocator = memoryAllocator; mMemoryAllocator = memoryAllocator;
} }
} }
#endif

155
ephysics/collision/narrowphase/EPA/EdgeEPA.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.h> #include <ephysics/collision/narrowphase/EPA/EdgeEPA.h>
@ -35,19 +16,19 @@ using namespace reactphysics3d;
// Constructor // Constructor
EdgeEPA::EdgeEPA() { EdgeEPA::EdgeEPA() {
} }
// Constructor // Constructor
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index) EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int32_t index)
: mOwnerTriangle(ownerTriangle), mIndex(index) { : mOwnerTriangle(ownerTriangle), mIndex(index) {
assert(index >= 0 && index < 3); assert(index >= 0 && index < 3);
} }
// Copy-constructor // Copy-constructor
EdgeEPA::EdgeEPA(const EdgeEPA& edge) { EdgeEPA::EdgeEPA(const EdgeEPA& edge) {
mOwnerTriangle = edge.mOwnerTriangle; mOwnerTriangle = edge.mOwnerTriangle;
mIndex = edge.mIndex; mIndex = edge.mIndex;
} }
// Destructor // Destructor
@ -56,81 +37,81 @@ EdgeEPA::~EdgeEPA() {
} }
// Return the index of the source vertex of the edge (vertex starting the edge) // Return the index of the source vertex of the edge (vertex starting the edge)
uint EdgeEPA::getSourceVertexIndex() const { uint32_t EdgeEPA::getSourceVertexIndex() const {
return (*mOwnerTriangle)[mIndex]; return (*mOwnerTriangle)[mIndex];
} }
// Return the index of the target vertex of the edge (vertex ending the edge) // Return the index of the target vertex of the edge (vertex ending the edge)
uint EdgeEPA::getTargetVertexIndex() const { uint32_t EdgeEPA::getTargetVertexIndex() const {
return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)]; return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)];
} }
// Execute the recursive silhouette algorithm from this edge // Execute the recursive silhouette algorithm from this edge
bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) { TrianglesStore& triangleStore) {
// If the edge has not already been visited // If the edge has not already been visited
if (!mOwnerTriangle->getIsObsolete()) { if (!mOwnerTriangle->getIsObsolete()) {
// If the triangle of this edge is not visible from the given point // If the triangle of this edge is not visible from the given point
if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
// If the triangle has been created // If the triangle has been created
if (triangle != NULL) { if (triangle != NULL) {
halfLink(EdgeEPA(triangle, 1), *this); halfLink(EdgeEPA(triangle, 1), *this);
return true; return true;
} }
return false; return false;
} }
else { else {
// The current triangle is visible and therefore obsolete // The current triangle is visible and therefore obsolete
mOwnerTriangle->setIsObsolete(true); mOwnerTriangle->setIsObsolete(true);
int backup = triangleStore.getNbTriangles(); int32_t backup = triangleStore.getNbTriangles();
if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
this->mIndex)).computeSilhouette(vertices, this->mIndex)).computeSilhouette(vertices,
indexNewVertex, indexNewVertex,
triangleStore)) { triangleStore)) {
mOwnerTriangle->setIsObsolete(false); mOwnerTriangle->setIsObsolete(false);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
// If the triangle has been created // If the triangle has been created
if (triangle != NULL) { if (triangle != NULL) {
halfLink(EdgeEPA(triangle, 1), *this); halfLink(EdgeEPA(triangle, 1), *this);
return true; return true;
} }
return false; return false;
} }
else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(
this->mIndex)).computeSilhouette(vertices, this->mIndex)).computeSilhouette(vertices,
indexNewVertex, indexNewVertex,
triangleStore)) { triangleStore)) {
mOwnerTriangle->setIsObsolete(false); mOwnerTriangle->setIsObsolete(false);
triangleStore.setNbTriangles(backup); triangleStore.setNbTriangles(backup);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
if (triangle != NULL) { if (triangle != NULL) {
halfLink(EdgeEPA(triangle, 1), *this); halfLink(EdgeEPA(triangle, 1), *this);
return true; return true;
} }
return false; return false;
} }
} }
} }
return true; return true;
} }

112
ephysics/collision/narrowphase/EPA/EdgeEPA.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_EDGE_EPA_H
#define REACTPHYSICS3D_EDGE_EPA_H
// Libraries // Libraries
@ -43,80 +22,79 @@ class TrianglesStore;
*/ */
class EdgeEPA { class EdgeEPA {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the triangle that contains this edge /// Pointer to the triangle that contains this edge
TriangleEPA* mOwnerTriangle; TriangleEPA* mOwnerTriangle;
/// Index of the edge in the triangle (between 0 and 2). /// Index of the edge in the triangle (between 0 and 2).
/// The edge with index i connect triangle vertices i and (i+1 % 3) /// The edge with index i connect triangle vertices i and (i+1 % 3)
int mIndex; int32_t mIndex;
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
EdgeEPA(); EdgeEPA();
/// Constructor /// Constructor
EdgeEPA(TriangleEPA* ownerTriangle, int index); EdgeEPA(TriangleEPA* ownerTriangle, int32_t index);
/// Copy-constructor /// Copy-constructor
EdgeEPA(const EdgeEPA& edge); EdgeEPA(const EdgeEPA& edge);
/// Destructor /// Destructor
~EdgeEPA(); ~EdgeEPA();
/// Return the pointer to the owner triangle /// Return the pointer to the owner triangle
TriangleEPA* getOwnerTriangle() const; TriangleEPA* getOwnerTriangle() const;
/// Return the index of the edge in the triangle /// Return the index of the edge in the triangle
int getIndex() const; int32_t getIndex() const;
/// Return index of the source vertex of the edge /// Return index of the source vertex of the edge
uint getSourceVertexIndex() const; uint32_t getSourceVertexIndex() const;
/// Return the index of the target vertex of the edge /// Return the index of the target vertex of the edge
uint getTargetVertexIndex() const; uint32_t getTargetVertexIndex() const;
/// Execute the recursive silhouette algorithm from this edge /// Execute the recursive silhouette algorithm from this edge
bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore);
/// Assignment operator /// Assignment operator
EdgeEPA& operator=(const EdgeEPA& edge); EdgeEPA& operator=(const EdgeEPA& edge);
}; };
// Return the pointer to the owner triangle // Return the pointer to the owner triangle
inline TriangleEPA* EdgeEPA::getOwnerTriangle() const { inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
return mOwnerTriangle; return mOwnerTriangle;
} }
// Return the edge index // Return the edge index
inline int EdgeEPA::getIndex() const { inline int32_t EdgeEPA::getIndex() const {
return mIndex; return mIndex;
} }
// Assignment operator // Assignment operator
inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) { inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
mOwnerTriangle = edge.mOwnerTriangle; mOwnerTriangle = edge.mOwnerTriangle;
mIndex = edge.mIndex; mIndex = edge.mIndex;
return *this; return *this;
} }
// Return the index of the next counter-clockwise edge of the ownver triangle // Return the index of the next counter-clockwise edge of the ownver triangle
inline int indexOfNextCounterClockwiseEdge(int i) { inline int32_t indexOfNextCounterClockwiseEdge(int32_t i) {
return (i + 1) % 3; return (i + 1) % 3;
} }
// Return the index of the previous counter-clockwise edge of the ownver triangle // Return the index of the previous counter-clockwise edge of the ownver triangle
inline int indexOfPreviousCounterClockwiseEdge(int i) { inline int32_t indexOfPreviousCounterClockwiseEdge(int32_t i) {
return (i + 2) % 3; return (i + 2) % 3;
} }
} }
#endif

156
ephysics/collision/narrowphase/EPA/TriangleEPA.cpp
View File

@ -1,28 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.h> #include <ephysics/collision/narrowphase/EPA/TriangleEPA.h>
@ -34,15 +14,15 @@ using namespace reactphysics3d;
// Constructor // Constructor
TriangleEPA::TriangleEPA() { TriangleEPA::TriangleEPA() {
} }
// Constructor // Constructor
TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3) TriangleEPA::TriangleEPA(uint32_t indexVertex1, uint32_t indexVertex2, uint32_t indexVertex3)
: mIsObsolete(false) { : mIsObsolete(false) {
mIndicesVertices[0] = indexVertex1; mIndicesVertices[0] = indexVertex1;
mIndicesVertices[1] = indexVertex2; mIndicesVertices[1] = indexVertex2;
mIndicesVertices[2] = indexVertex3; mIndicesVertices[2] = indexVertex3;
} }
// Destructor // Destructor
@ -52,50 +32,50 @@ TriangleEPA::~TriangleEPA() {
// Compute the point v closest to the origin of this triangle // Compute the point v closest to the origin of this triangle
bool TriangleEPA::computeClosestPoint(const Vector3* vertices) { bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
const Vector3& p0 = vertices[mIndicesVertices[0]]; const Vector3& p0 = vertices[mIndicesVertices[0]];
Vector3 v1 = vertices[mIndicesVertices[1]] - p0; Vector3 v1 = vertices[mIndicesVertices[1]] - p0;
Vector3 v2 = vertices[mIndicesVertices[2]] - p0; Vector3 v2 = vertices[mIndicesVertices[2]] - p0;
decimal v1Dotv1 = v1.dot(v1); float v1Dotv1 = v1.dot(v1);
decimal v1Dotv2 = v1.dot(v2); float v1Dotv2 = v1.dot(v2);
decimal v2Dotv2 = v2.dot(v2); float v2Dotv2 = v2.dot(v2);
decimal p0Dotv1 = p0.dot(v1); float p0Dotv1 = p0.dot(v1);
decimal p0Dotv2 = p0.dot(v2); float p0Dotv2 = p0.dot(v2);
// Compute determinant // Compute determinant
mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
// Compute lambda values // Compute lambda values
mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2;
mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1;
// If the determinant is positive // If the determinant is positive
if (mDet > 0.0) { if (mDet > 0.0) {
// Compute the closest point v // Compute the closest point v
mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2); mClosestPoint = p0 + float(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2);
// Compute the square distance of closest point to the origin // Compute the square distance of closest point to the origin
mDistSquare = mClosestPoint.dot(mClosestPoint); mDistSquare = mClosestPoint.dot(mClosestPoint);
return true; return true;
} }
return false; return false;
} }
/// Link an edge with another one. It means that the current edge of a triangle will /// 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 /// be associated with the edge of another triangle in order that both triangles
/// are neighbour along both edges). /// are neighbour along both edges).
bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) { bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
if (isPossible) { if (isPossible) {
edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0; edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0;
} }
return isPossible; return isPossible;
} }
/// Make an half link of an edge with another one from another triangle. An half-link /// Make an half link of an edge with another one from another triangle. An half-link
@ -103,11 +83,11 @@ bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will /// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
/// be made later. /// be made later.
void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) { void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
// Link // Link
edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
} }
// Execute the recursive silhouette algorithm from this triangle face. // Execute the recursive silhouette algorithm from this triangle face.
@ -121,35 +101,35 @@ void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in /// 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 /// 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. /// obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) { TrianglesStore& triangleStore) {
uint first = triangleStore.getNbTriangles(); uint32_t first = triangleStore.getNbTriangles();
// Mark the current triangle as obsolete because it // Mark the current triangle as obsolete because it
setIsObsolete(true); setIsObsolete(true);
// Execute recursively the silhouette algorithm for the adjacent edges of neighboring // Execute recursively the silhouette algorithm for the adjacent edges of neighboring
// triangles of the current triangle // triangles of the current triangle
bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) && mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
if (result) { if (result) {
int i,j; int32_t i,j;
// For each triangle face that contains the new vertex and an edge of the silhouette // For each triangle face that contains the new vertex and an edge of the silhouette
for (i=first, j=triangleStore.getNbTriangles()-1; for (i=first, j=triangleStore.getNbTriangles()-1;
i != triangleStore.getNbTriangles(); j = i++) { i != triangleStore.getNbTriangles(); j = i++) {
TriangleEPA* triangle = &triangleStore[i]; TriangleEPA* triangle = &triangleStore[i];
halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1)); halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));
if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) { if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) {
return false; return false;
} }
} }
} }
return result; return result;
} }

195
ephysics/collision/narrowphase/EPA/TriangleEPA.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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 // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -46,154 +25,152 @@ void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
*/ */
class TriangleEPA { class TriangleEPA {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Indices of the vertices y_i of the triangle /// Indices of the vertices y_i of the triangle
uint mIndicesVertices[3]; uint32_t mIndicesVertices[3];
/// Three adjacent edges of the triangle (edges of other triangles) /// Three adjacent edges of the triangle (edges of other triangles)
EdgeEPA mAdjacentEdges[3]; EdgeEPA mAdjacentEdges[3];
/// True if the triangle face is visible from the new support point /// True if the triangle face is visible from the new support point
bool mIsObsolete; bool mIsObsolete;
/// Determinant /// Determinant
decimal mDet; float mDet;
/// Point v closest to the origin on the affine hull of the triangle /// Point v closest to the origin on the affine hull of the triangle
Vector3 mClosestPoint; Vector3 mClosestPoint;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal mLambda1; float mLambda1;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal mLambda2; float mLambda2;
/// Square distance of the point closest point v to the origin /// Square distance of the point closest point v to the origin
decimal mDistSquare; float mDistSquare;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
TriangleEPA(const TriangleEPA& triangle); TriangleEPA(const TriangleEPA& triangle);
/// Private assignment operator /// Private assignment operator
TriangleEPA& operator=(const TriangleEPA& triangle); TriangleEPA& operator=(const TriangleEPA& triangle);
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
TriangleEPA(); TriangleEPA();
/// Constructor /// Constructor
TriangleEPA(uint v1, uint v2, uint v3); TriangleEPA(uint32_t v1, uint32_t v2, uint32_t v3);
/// Destructor /// Destructor
~TriangleEPA(); ~TriangleEPA();
/// Return an adjacent edge of the triangle /// Return an adjacent edge of the triangle
EdgeEPA& getAdjacentEdge(int index); EdgeEPA& getAdjacentEdge(int32_t index);
/// Set an adjacent edge of the triangle /// Set an adjacent edge of the triangle
void setAdjacentEdge(int index, EdgeEPA& edge); void setAdjacentEdge(int32_t index, EdgeEPA& edge);
/// Return the square distance of the closest point to origin /// Return the square distance of the closest point to origin
decimal getDistSquare() const; float getDistSquare() const;
/// Set the isObsolete value /// Set the isObsolete value
void setIsObsolete(bool isObsolete); void setIsObsolete(bool isObsolete);
/// Return true if the triangle face is obsolete /// Return true if the triangle face is obsolete
bool getIsObsolete() const; bool getIsObsolete() const;
/// Return the point closest to the origin /// Return the point closest to the origin
const Vector3& getClosestPoint() const; const Vector3& getClosestPoint() const;
// Return true if the closest point on affine hull is inside the triangle // Return true if the closest point on affine hull is inside the triangle
bool isClosestPointInternalToTriangle() const; bool isClosestPointInternalToTriangle() const;
/// Return true if the triangle is visible from a given vertex /// Return true if the triangle is visible from a given vertex
bool isVisibleFromVertex(const Vector3* vertices, uint index) const; bool isVisibleFromVertex(const Vector3* vertices, uint32_t index) const;
/// Compute the point v closest to the origin of this triangle /// Compute the point v closest to the origin of this triangle
bool computeClosestPoint(const Vector3* vertices); bool computeClosestPoint(const Vector3* vertices);
/// Compute the point of an object closest to the origin /// Compute the point of an object closest to the origin
Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const;
/// Execute the recursive silhouette algorithm from this triangle face. /// Execute the recursive silhouette algorithm from this triangle face.
bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore);
/// Access operator /// Access operator
uint operator[](int i) const; uint32_t operator[](int32_t i) const;
/// Associate two edges /// Associate two edges
friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
/// Make a half-link between two edges /// Make a half-link between two edges
friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
}; };
// Return an edge of the triangle // Return an edge of the triangle
inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) { inline EdgeEPA& TriangleEPA::getAdjacentEdge(int32_t index) {
assert(index >= 0 && index < 3); assert(index >= 0 && index < 3);
return mAdjacentEdges[index]; return mAdjacentEdges[index];
} }
// Set an adjacent edge of the triangle // Set an adjacent edge of the triangle
inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) { inline void TriangleEPA::setAdjacentEdge(int32_t index, EdgeEPA& edge) {
assert(index >=0 && index < 3); assert(index >=0 && index < 3);
mAdjacentEdges[index] = edge; mAdjacentEdges[index] = edge;
} }
// Return the square distance of the closest point to origin // Return the square distance of the closest point to origin
inline decimal TriangleEPA::getDistSquare() const { inline float TriangleEPA::getDistSquare() const {
return mDistSquare; return mDistSquare;
} }
// Set the isObsolete value // Set the isObsolete value
inline void TriangleEPA::setIsObsolete(bool isObsolete) { inline void TriangleEPA::setIsObsolete(bool isObsolete) {
mIsObsolete = isObsolete; mIsObsolete = isObsolete;
} }
// Return true if the triangle face is obsolete // Return true if the triangle face is obsolete
inline bool TriangleEPA::getIsObsolete() const { inline bool TriangleEPA::getIsObsolete() const {
return mIsObsolete; return mIsObsolete;
} }
// Return the point closest to the origin // Return the point closest to the origin
inline const Vector3& TriangleEPA::getClosestPoint() const { inline const Vector3& TriangleEPA::getClosestPoint() const {
return mClosestPoint; return mClosestPoint;
} }
// Return true if the closest point on affine hull is inside the triangle // Return true if the closest point on affine hull is inside the triangle
inline bool TriangleEPA::isClosestPointInternalToTriangle() const { inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet); return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet);
} }
// Return true if the triangle is visible from a given vertex // Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const { inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint32_t index) const {
Vector3 closestToVert = vertices[index] - mClosestPoint; Vector3 closestToVert = vertices[index] - mClosestPoint;
return (mClosestPoint.dot(closestToVert) > 0.0); return (mClosestPoint.dot(closestToVert) > 0.0);
} }
// Compute the point of an object closest to the origin // Compute the point of an object closest to the origin
inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{ inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{
const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]]; const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]];
return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) + return p0 + float(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0)); mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
} }
// Access operator // Access operator
inline uint TriangleEPA::operator[](int i) const { inline uint32_t TriangleEPA::operator[](int32_t i) const {
assert(i >= 0 && i <3); assert(i >= 0 && i <3);
return mIndicesVertices[i]; return mIndicesVertices[i];
} }
} }
#endif

31
ephysics/collision/narrowphase/EPA/TrianglesStore.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.h> #include <ephysics/collision/narrowphase/EPA/TrianglesStore.h>
@ -31,7 +12,7 @@ using namespace reactphysics3d;
// Constructor // Constructor
TrianglesStore::TrianglesStore() : mNbTriangles(0) { TrianglesStore::TrianglesStore() : mNbTriangles(0) {
} }
// Destructor // Destructor

140
ephysics/collision/narrowphase/EPA/TrianglesStore.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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 <ephysics/collision/narrowphase/EPA/TriangleEPA.h> #include <ephysics/collision/narrowphase/EPA/TriangleEPA.h>
@ -36,7 +15,7 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
const unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles const uint32_t MAX_TRIANGLES = 200; // Maximum number of triangles
// Class TriangleStore // Class TriangleStore
/** /**
@ -44,98 +23,97 @@ const unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles
*/ */
class TrianglesStore { class TrianglesStore {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Triangles /// Triangles
TriangleEPA mTriangles[MAX_TRIANGLES]; TriangleEPA mTriangles[MAX_TRIANGLES];
/// Number of triangles /// Number of triangles
int mNbTriangles; int32_t mNbTriangles;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
TrianglesStore(const TrianglesStore& triangleStore); TrianglesStore(const TrianglesStore& triangleStore);
/// Private assignment operator /// Private assignment operator
TrianglesStore& operator=(const TrianglesStore& triangleStore); TrianglesStore& operator=(const TrianglesStore& triangleStore);
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
TrianglesStore(); TrianglesStore();
/// Destructor /// Destructor
~TrianglesStore(); ~TrianglesStore();
/// Clear all the storage /// Clear all the storage
void clear(); void clear();
/// Return the number of triangles /// Return the number of triangles
int getNbTriangles() const; int32_t getNbTriangles() const;
/// Set the number of triangles /// Set the number of triangles
void setNbTriangles(int backup); void setNbTriangles(int32_t backup);
/// Return the last triangle /// Return the last triangle
TriangleEPA& last(); TriangleEPA& last();
/// Create a new triangle /// Create a new triangle
TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2); TriangleEPA* newTriangle(const Vector3* vertices, uint32_t v0, uint32_t v1, uint32_t v2);
/// Access operator /// Access operator
TriangleEPA& operator[](int i); TriangleEPA& operator[](int32_t i);
}; };
// Clear all the storage // Clear all the storage
inline void TrianglesStore::clear() { inline void TrianglesStore::clear() {
mNbTriangles = 0; mNbTriangles = 0;
} }
// Return the number of triangles // Return the number of triangles
inline int TrianglesStore::getNbTriangles() const { inline int32_t TrianglesStore::getNbTriangles() const {
return mNbTriangles; return mNbTriangles;
} }
inline void TrianglesStore::setNbTriangles(int backup) { inline void TrianglesStore::setNbTriangles(int32_t backup) {
mNbTriangles = backup; mNbTriangles = backup;
} }
// Return the last triangle // Return the last triangle
inline TriangleEPA& TrianglesStore::last() { inline TriangleEPA& TrianglesStore::last() {
assert(mNbTriangles > 0); assert(mNbTriangles > 0);
return mTriangles[mNbTriangles - 1]; return mTriangles[mNbTriangles - 1];
} }
// Create a new triangle // Create a new triangle
inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices,
uint v0,uint v1, uint v2) { uint32_t v0,uint32_t v1, uint32_t v2) {
TriangleEPA* newTriangle = NULL; TriangleEPA* newTriangle = NULL;
// If we have not reached the maximum number of triangles // If we have not reached the maximum number of triangles
if (mNbTriangles != MAX_TRIANGLES) { if (mNbTriangles != MAX_TRIANGLES) {
newTriangle = &mTriangles[mNbTriangles++]; newTriangle = &mTriangles[mNbTriangles++];
new (newTriangle) TriangleEPA(v0, v1, v2); new (newTriangle) TriangleEPA(v0, v1, v2);
if (!newTriangle->computeClosestPoint(vertices)) { if (!newTriangle->computeClosestPoint(vertices)) {
mNbTriangles--; mNbTriangles--;
newTriangle = NULL; newTriangle = NULL;
} }
} }
// Return the new triangle // Return the new triangle
return newTriangle; return newTriangle;
} }
// Access operator // Access operator
inline TriangleEPA& TrianglesStore::operator[](int i) { inline TriangleEPA& TrianglesStore::operator[](int32_t i) {
return mTriangles[i]; return mTriangles[i];
} }
} }
#endif

705
ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.h> #include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.h>
@ -49,375 +30,375 @@ GJKAlgorithm::~GJKAlgorithm() {
// Compute a contact info if the two collision shapes collide. // Compute a contact info if the two collision shapes collide.
/// This method implements the Hybrid Technique for computing the penetration depth by /// 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 /// running the GJK algorithm on original objects (without margin). If the shapes int32_tersect
/// only in the margins, the method compute the penetration depth and contact points /// only in the margins, the method compute the penetration depth and contact points
/// (of enlarged objects). If the original objects (without margin) intersect, we /// (of enlarged objects). If the original objects (without margin) int32_tersect, we
/// call the computePenetrationDepthForEnlargedObjects() method that run the GJK /// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
/// algorithm on the enlarged object to obtain a simplex polytope that contains the /// 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 /// 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. /// the correct penetration depth and contact points between the enlarged objects.
void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("GJKAlgorithm::testCollision()"); PROFILE("GJKAlgorithm::testCollision()");
Vector3 suppA; // Support point of object A Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B Vector3 suppB; // Support point of object B
Vector3 w; // Support point of Minkowski difference A-B Vector3 w; // Support point of Minkowski difference A-B
Vector3 pA; // Closest point of object A Vector3 pA; // Closest point of object A
Vector3 pB; // Closest point of object B Vector3 pB; // Closest point of object B
decimal vDotw; float vDotw;
decimal prevDistSquare; float prevDistSquare;
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape); const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape); const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// Get the local-space to world-space transforms // Get the local-space to world-space transforms
const Transform transform1 = shape1Info.shapeToWorldTransform; const Transform transform1 = shape1Info.shapeToWorldTransform;
const Transform transform2 = shape2Info.shapeToWorldTransform; const Transform transform2 = shape2Info.shapeToWorldTransform;
// Transform a point from local space of body 2 to local // 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) // space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2; Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local // Matrix that transform a direction from local
// space of body 1 into local space of body 2 // space of body 1 int32_to local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix(); transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects) // Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin(); float margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin; float marginSquare = margin * margin;
assert(margin > 0.0); assert(margin > 0.0);
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
// Get the previous point V (last cached separating axis) // Get the previous point V (last cached separating axis)
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST; float distSquare = DECIMAL_LARGEST;
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
suppB = body2Tobody1 * suppB = body2Tobody1 *
shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
vDotw = v.dot(w); vDotw = v.dot(w);
// If the enlarge objects (with margins) do not intersect // If the enlarge objects (with margins) do not int32_tersect
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence // Cache the current separating axis for frame coherence
mCurrentOverlappingPair->setCachedSeparatingAxis(v); mCurrentOverlappingPair->setCachedSeparatingAxis(v);
// No intersection, we return // No int32_tersection, we return
return; return;
} }
// If the objects intersect only in the margins // If the objects int32_tersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
decimal dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit()); Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
// Add the new support point to the simplex // Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent // If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) { if (simplex.isAffinelyDependent()) {
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
decimal dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit()); Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
// Compute the point of the simplex closest to the origin // 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 fail
if (!simplex.computeClosestPoint(v)) { if (!simplex.computeClosestPoint(v)) {
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
decimal dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit()); Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
// Store and update the squared distance of the closest point // Store and update the squared distance of the closest point
prevDistSquare = distSquare; prevDistSquare = distSquare;
distSquare = v.lengthSquare(); distSquare = v.lengthSquare();
// If the distance to the closest point doesn't improve a lot // If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v); simplex.backupClosestPointInSimplex(v);
// Get the new squared distance // Get the new squared distance
distSquare = v.lengthSquare(); distSquare = v.lengthSquare();
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
decimal dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit()); Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint()); simplex.getMaxLengthSquareOfAPoint());
// The objects (without margins) intersect. Therefore, we run the GJK algorithm // The objects (without margins) int32_tersect. Therefore, we run the GJK algorithm
// again but on the enlarged objects to compute a simplex polytope that contains // 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 origin. Then, we give that simplex polytope to the EPA algorithm to compute
// the correct penetration depth and contact points between the enlarged objects. // the correct penetration depth and contact points between the enlarged objects.
return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
transform2, narrowPhaseCallback, v); transform2, narrowPhaseCallback, v);
} }
/// This method runs the GJK algorithm on the two enlarged objects (with margin) /// 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 /// to compute a simplex polytope that contains the origin. The two objects are
/// assumed to intersect in the original objects (without margin). Therefore such /// assumed to int32_tersect in the original objects (without margin). Therefore such
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to /// 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. /// compute the correct penetration depth and contact points of the enlarged objects.
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1, const Transform& transform1,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
const Transform& transform2, const Transform& transform2,
NarrowPhaseCallback* narrowPhaseCallback, NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v) { Vector3& v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
Simplex simplex; Simplex simplex;
Vector3 suppA; Vector3 suppA;
Vector3 suppB; Vector3 suppB;
Vector3 w; Vector3 w;
decimal vDotw; float vDotw;
decimal distSquare = DECIMAL_LARGEST; float distSquare = DECIMAL_LARGEST;
decimal prevDistSquare; float prevDistSquare;
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape); const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape); const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// Transform a point from local space of body 2 to local space // 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) // of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2ToBody1 = transform1.getInverse() * transform2; Transform body2ToBody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local space of body 1 into local space of body 2 // Matrix that transform a direction from local space of body 1 int32_to local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix(); transform1.getOrientation().getMatrix();
do { do {
// Compute the support points for the enlarged object A and B // Compute the support points for the enlarged object A and B
suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
vDotw = v.dot(w); vDotw = v.dot(w);
// If the enlarge objects do not intersect // If the enlarge objects do not int32_tersect
if (vDotw > 0.0) { if (vDotw > 0.0) {
// No intersection, we return // No int32_tersection, we return
return; return;
} }
// Add the new support point to the simplex // Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
if (simplex.isAffinelyDependent()) { if (simplex.isAffinelyDependent()) {
return; return;
} }
if (!simplex.computeClosestPoint(v)) { if (!simplex.computeClosestPoint(v)) {
return; return;
} }
// Store and update the square distance // Store and update the square distance
prevDistSquare = distSquare; prevDistSquare = distSquare;
distSquare = v.lengthSquare(); distSquare = v.lengthSquare();
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return; return;
} }
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint()); simplex.getMaxLengthSquareOfAPoint());
// Give the simplex computed with GJK algorithm to the EPA algorithm // Give the simplex computed with GJK algorithm to the EPA algorithm
// which will compute the correct penetration depth and contact points // which will compute the correct penetration depth and contact points
// between the two enlarged objects // between the two enlarged objects
return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
transform1, shape2Info, transform2, transform1, shape2Info, transform2,
v, narrowPhaseCallback); v, narrowPhaseCallback);
} }
// Use the GJK Algorithm to find if a point is inside a convex collision shape // Use the GJK Algorithm to find if a point is inside a convex collision shape
bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) { bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
Vector3 suppA; // Support point of object A Vector3 suppA; // Support point of object A
Vector3 w; // Support point of Minkowski difference A-B Vector3 w; // Support point of Minkowski difference A-B
decimal prevDistSquare; float prevDistSquare;
assert(proxyShape->getCollisionShape()->isConvex()); assert(proxyShape->getCollisionShape()->isConvex());
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape()); const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
// Support point of object B (object B is a single point) // Support point of object B (object B is a single point)
const Vector3 suppB(localPoint); const Vector3 suppB(localPoint);
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
// Initial supporting direction // Initial supporting direction
Vector3 v(1, 1, 1); Vector3 v(1, 1, 1);
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST; float distSquare = DECIMAL_LARGEST;
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
// Add the new support point to the simplex // Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent // If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) { if (simplex.isAffinelyDependent()) {
return false; return false;
} }
// Compute the point of the simplex closest to the origin // 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 fail
if (!simplex.computeClosestPoint(v)) { if (!simplex.computeClosestPoint(v)) {
return false; return false;
} }
// Store and update the squared distance of the closest point // Store and update the squared distance of the closest point
prevDistSquare = distSquare; prevDistSquare = distSquare;
distSquare = v.lengthSquare(); distSquare = v.lengthSquare();
// If the distance to the closest point doesn't improve a lot // If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return false; return false;
} }
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint()); simplex.getMaxLengthSquareOfAPoint());
// The point is inside the collision shape // The point is inside the collision shape
return true; return true;
} }
@ -426,103 +407,103 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection". /// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) { bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) {
assert(proxyShape->getCollisionShape()->isConvex()); assert(proxyShape->getCollisionShape()->isConvex());
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape()); const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape Vector3 suppB; // Support point on the collision shape
const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; const float machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
const decimal epsilon = decimal(0.0001); const float epsilon = float(0.0001);
// Convert the ray origin and direction into the local-space of the collision shape // Convert the ray origin and direction int32_to the local-space of the collision shape
Vector3 rayDirection = ray.point2 - ray.point1; Vector3 rayDirection = ray.point2 - ray.point1;
// If the points of the segment are two close, return no hit // If the points of the segment are two close, return no hit
if (rayDirection.lengthSquare() < machineEpsilonSquare) return false; if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
Vector3 w; Vector3 w;
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); Vector3 n(float(0.0), float(0.0), float(0.0));
decimal lambda = decimal(0.0); float lambda = float(0.0);
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
Vector3 v = suppA - suppB; Vector3 v = suppA - suppB;
decimal vDotW, vDotR; float vDotW, vDotR;
decimal distSquare = v.lengthSquare(); float distSquare = v.lengthSquare();
int nbIterations = 0; int32_t nbIterations = 0;
// GJK Algorithm loop // GJK Algorithm loop
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
// Compute the support points // Compute the support points
suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
w = suppA - suppB; w = suppA - suppB;
vDotW = v.dot(w); vDotW = v.dot(w);
if (vDotW > decimal(0)) { if (vDotW > float(0)) {
vDotR = v.dot(rayDirection); vDotR = v.dot(rayDirection);
if (vDotR >= -machineEpsilonSquare) { if (vDotR >= -machineEpsilonSquare) {
return false; return false;
} }
else { else {
// We have found a better lower bound for the hit point along the ray // We have found a better lower bound for the hit point along the ray
lambda = lambda - vDotW / vDotR; lambda = lambda - vDotW / vDotR;
suppA = ray.point1 + lambda * rayDirection; suppA = ray.point1 + lambda * rayDirection;
w = suppA - suppB; w = suppA - suppB;
n = v; n = v;
} }
} }
// Add the new support point to the simplex // Add the new support point to the simplex
if (!simplex.isPointInSimplex(w)) { if (!simplex.isPointInSimplex(w)) {
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
} }
// Compute the closest point // Compute the closest point
if (simplex.computeClosestPoint(v)) { if (simplex.computeClosestPoint(v)) {
distSquare = v.lengthSquare(); distSquare = v.lengthSquare();
} }
else { else {
distSquare = decimal(0.0); distSquare = float(0.0);
} }
// If the current lower bound distance is larger than the maximum raycasting distance // If the current lower bound distance is larger than the maximum raycasting distance
if (lambda > ray.maxFraction) return false; if (lambda > ray.maxFraction) return false;
nbIterations++; nbIterations++;
} }
// If the origin was inside the shape, we return no hit // If the origin was inside the shape, we return no hit
if (lambda < MACHINE_EPSILON) return false; if (lambda < MACHINE_EPSILON) return false;
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
Vector3 pointA; Vector3 pointA;
Vector3 pointB; Vector3 pointB;
simplex.computeClosestPointsOfAandB(pointA, pointB); simplex.computeClosestPointsOfAandB(pointA, pointB);
// A raycast hit has been found, we fill in the raycast info // A raycast hit has been found, we fill in the raycast info
raycastInfo.hitFraction = lambda; raycastInfo.hitFraction = lambda;
raycastInfo.worldPoint = pointB; raycastInfo.worldPoint = pointB;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = n; raycastInfo.worldNormal = n;
} }
else { // Degenerated normal vector, we return a zero normal vector else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0)); raycastInfo.worldNormal = Vector3(float(0), float(0), float(0));
} }
return true; return true;
} }

116
ephysics/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_GJK_ALGORITHM_H
#define REACTPHYSICS3D_GJK_ALGORITHM_H
// Libraries // Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -37,9 +16,9 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
const decimal REL_ERROR = decimal(1.0e-3); const float REL_ERROR = float(1.0e-3);
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; const float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const int MAX_ITERATIONS_GJK_RAYCAST = 32; const int32_t MAX_ITERATIONS_GJK_RAYCAST = 32;
// Class GJKAlgorithm // Class GJKAlgorithm
/** /**
@ -49,9 +28,9 @@ const int MAX_ITERATIONS_GJK_RAYCAST = 32;
* "Collision Detection in Interactive 3D Environments" by Gino van den Bergen. * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen.
* This method implements the Hybrid Technique for calculating the * This method implements the Hybrid Technique for calculating the
* penetration depth. The two objects are enlarged with a small margin. If * penetration depth. The two objects are enlarged with a small margin. If
* the object intersects in their margins, the penetration depth is quickly * the object int32_tersects in their margins, the penetration depth is quickly
* computed using the GJK algorithm on the original objects (without margin). * computed using the GJK algorithm on the original objects (without margin).
* If the original objects (without margin) intersect, we run again the GJK * If the original objects (without margin) int32_tersect, we run again the GJK
* algorithm on the enlarged objects (with margin) to compute simplex * algorithm on the enlarged objects (with margin) to compute simplex
* polytope that contains the origin and give it to the EPA (Expanding * polytope that contains the origin and give it to the EPA (Expanding
* Polytope Algorithm) to compute the correct penetration depth between the * Polytope Algorithm) to compute the correct penetration depth between the
@ -59,62 +38,61 @@ const int MAX_ITERATIONS_GJK_RAYCAST = 32;
*/ */
class GJKAlgorithm : public NarrowPhaseAlgorithm { class GJKAlgorithm : public NarrowPhaseAlgorithm {
private : private :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// EPA Algorithm /// EPA Algorithm
EPAAlgorithm mAlgoEPA; EPAAlgorithm mAlgoEPA;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
GJKAlgorithm(const GJKAlgorithm& algorithm); GJKAlgorithm(const GJKAlgorithm& algorithm);
/// Private assignment operator /// Private assignment operator
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm); GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
/// Compute the penetration depth for enlarged objects. /// Compute the penetration depth for enlarged objects.
void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1, const Transform& transform1,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
const Transform& transform2, const Transform& transform2,
NarrowPhaseCallback* narrowPhaseCallback, NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v); Vector3& v);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
GJKAlgorithm(); GJKAlgorithm();
/// Destructor /// Destructor
~GJKAlgorithm(); ~GJKAlgorithm();
/// Initalize the algorithm /// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection, virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator); MemoryAllocator* memoryAllocator);
/// Compute a contact info if the two bounding volumes collide. /// Compute a contact info if the two bounding volumes collide.
virtual void testCollision(const CollisionShapeInfo& shape1Info, virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape /// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
}; };
// Initalize the algorithm // Initalize the algorithm
inline void GJKAlgorithm::init(CollisionDetection* collisionDetection, inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) { MemoryAllocator* memoryAllocator) {
NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator); NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
mAlgoEPA.init(memoryAllocator); mAlgoEPA.init(memoryAllocator);
} }
} }
#endif

529
ephysics/collision/narrowphase/GJK/Simplex.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.h> #include <ephysics/collision/narrowphase/GJK/Simplex.h>
@ -40,278 +21,278 @@ Simplex::~Simplex() {
} }
// Add a new support point of (A-B) into the simplex // Add a new support point of (A-B) int32_to the simplex
/// suppPointA : support point of object A in a direction -v /// suppPointA : support point of object A in a direction -v
/// suppPointB : support point of object B in a direction v /// suppPointB : support point of object B in a direction v
/// point : support point of object (A-B) => point = suppPointA - suppPointB /// point : support point of object (A-B) => point = suppPointA - suppPointB
void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) { void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) {
assert(!isFull()); assert(!isFull());
mLastFound = 0; mLastFound = 0;
mLastFoundBit = 0x1; mLastFoundBit = 0x1;
// Look for the bit corresponding to one of the four point that is not in // Look for the bit corresponding to one of the four point that is not in
// the current simplex // the current simplex
while (overlap(mBitsCurrentSimplex, mLastFoundBit)) { while (overlap(mBitsCurrentSimplex, mLastFoundBit)) {
mLastFound++; mLastFound++;
mLastFoundBit <<= 1; mLastFoundBit <<= 1;
} }
assert(mLastFound < 4); assert(mLastFound < 4);
// Add the point into the simplex // Add the point int32_to the simplex
mPoints[mLastFound] = point; mPoints[mLastFound] = point;
mPointsLengthSquare[mLastFound] = point.dot(point); mPointsLengthSquare[mLastFound] = point.dot(point);
mAllBits = mBitsCurrentSimplex | mLastFoundBit; mAllBits = mBitsCurrentSimplex | mLastFoundBit;
// Update the cached values // Update the cached values
updateCache(); updateCache();
// Compute the cached determinant values // Compute the cached determinant values
computeDeterminants(); computeDeterminants();
// Add the support points of objects A and B // Add the support points of objects A and B
mSuppPointsA[mLastFound] = suppPointA; mSuppPointsA[mLastFound] = suppPointA;
mSuppPointsB[mLastFound] = suppPointB; mSuppPointsB[mLastFound] = suppPointB;
} }
// Return true if the point is in the simplex // Return true if the point is in the simplex
bool Simplex::isPointInSimplex(const Vector3& point) const { bool Simplex::isPointInSimplex(const Vector3& point) const {
int i; int32_t i;
Bits bit; Bits bit;
// For each four possible points in the simplex // For each four possible points in the simplex
for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { for (i=0, bit = 0x1; i<4; i++, bit <<= 1) {
// Check if the current point is in the simplex // Check if the current point is in the simplex
if (overlap(mAllBits, bit) && point == mPoints[i]) return true; if (overlap(mAllBits, bit) && point == mPoints[i]) return true;
} }
return false; return false;
} }
// Update the cached values used during the GJK algorithm // Update the cached values used during the GJK algorithm
void Simplex::updateCache() { void Simplex::updateCache() {
int i; int32_t i;
Bits bit; Bits bit;
// For each of the four possible points of the simplex // For each of the four possible points of the simplex
for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { for (i=0, bit = 0x1; i<4; i++, bit <<= 1) {
// If the current points is in the simplex // If the current points is in the simplex
if (overlap(mBitsCurrentSimplex, bit)) { if (overlap(mBitsCurrentSimplex, bit)) {
// Compute the distance between two points in the possible simplex set // Compute the distance between two points in the possible simplex set
mDiffLength[i][mLastFound] = mPoints[i] - mPoints[mLastFound]; mDiffLength[i][mLastFound] = mPoints[i] - mPoints[mLastFound];
mDiffLength[mLastFound][i] = -mDiffLength[i][mLastFound]; mDiffLength[mLastFound][i] = -mDiffLength[i][mLastFound];
// Compute the squared length of the vector // Compute the squared length of the vector
// distances from points in the possible simplex set // distances from points in the possible simplex set
mNormSquare[i][mLastFound] = mNormSquare[mLastFound][i] = mNormSquare[i][mLastFound] = mNormSquare[mLastFound][i] =
mDiffLength[i][mLastFound].dot(mDiffLength[i][mLastFound]); mDiffLength[i][mLastFound].dot(mDiffLength[i][mLastFound]);
} }
} }
} }
// Return the points of the simplex // Return the points of the simplex
unsigned int Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, uint32_t Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB,
Vector3* points) const { Vector3* points) const {
unsigned int nbVertices = 0; uint32_t nbVertices = 0;
int i; int32_t i;
Bits bit; Bits bit;
// For each four point in the possible simplex // For each four point in the possible simplex
for (i=0, bit=0x1; i<4; i++, bit <<=1) { for (i=0, bit=0x1; i<4; i++, bit <<=1) {
// If the current point is in the simplex // If the current point is in the simplex
if (overlap(mBitsCurrentSimplex, bit)) { if (overlap(mBitsCurrentSimplex, bit)) {
// Store the points // Store the points
suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices]; suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices];
suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices]; suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices];
points[nbVertices] = this->mPoints[nbVertices]; points[nbVertices] = this->mPoints[nbVertices];
nbVertices++; nbVertices++;
} }
} }
// Return the number of points in the simplex // Return the number of points in the simplex
return nbVertices; return nbVertices;
} }
// Compute the cached determinant values // Compute the cached determinant values
void Simplex::computeDeterminants() { void Simplex::computeDeterminants() {
mDet[mLastFoundBit][mLastFound] = 1.0; mDet[mLastFoundBit][mLastFound] = 1.0;
// If the current simplex is not empty // If the current simplex is not empty
if (!isEmpty()) { if (!isEmpty()) {
int i; int32_t i;
Bits bitI; Bits bitI;
// For each possible four points in the simplex set // For each possible four points in the simplex set
for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) { for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) {
// If the current point is in the simplex // If the current point is in the simplex
if (overlap(mBitsCurrentSimplex, bitI)) { if (overlap(mBitsCurrentSimplex, bitI)) {
Bits bit2 = bitI | mLastFoundBit; Bits bit2 = bitI | mLastFoundBit;
mDet[bit2][i] = mDiffLength[mLastFound][i].dot(mPoints[mLastFound]); mDet[bit2][i] = mDiffLength[mLastFound][i].dot(mPoints[mLastFound]);
mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]); mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]);
int j; int32_t j;
Bits bitJ; Bits bitJ;
for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) { for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) {
if (overlap(mBitsCurrentSimplex, bitJ)) { if (overlap(mBitsCurrentSimplex, bitJ)) {
int k; int32_t k;
Bits bit3 = bitJ | bit2; Bits bit3 = bitJ | bit2;
k = mNormSquare[i][j] < mNormSquare[mLastFound][j] ? i : mLastFound; k = mNormSquare[i][j] < mNormSquare[mLastFound][j] ? i : mLastFound;
mDet[bit3][j] = mDet[bit2][i] * mDiffLength[k][j].dot(mPoints[i]) + mDet[bit3][j] = mDet[bit2][i] * mDiffLength[k][j].dot(mPoints[i]) +
mDet[bit2][mLastFound] * mDet[bit2][mLastFound] *
mDiffLength[k][j].dot(mPoints[mLastFound]); mDiffLength[k][j].dot(mPoints[mLastFound]);
k = mNormSquare[j][i] < mNormSquare[mLastFound][i] ? j : mLastFound; k = mNormSquare[j][i] < mNormSquare[mLastFound][i] ? j : mLastFound;
mDet[bit3][i] = mDet[bitJ | mLastFoundBit][j] * mDet[bit3][i] = mDet[bitJ | mLastFoundBit][j] *
mDiffLength[k][i].dot(mPoints[j]) + mDiffLength[k][i].dot(mPoints[j]) +
mDet[bitJ | mLastFoundBit][mLastFound] * mDet[bitJ | mLastFoundBit][mLastFound] *
mDiffLength[k][i].dot(mPoints[mLastFound]); mDiffLength[k][i].dot(mPoints[mLastFound]);
k = mNormSquare[i][mLastFound] < mNormSquare[j][mLastFound] ? i : j; k = mNormSquare[i][mLastFound] < mNormSquare[j][mLastFound] ? i : j;
mDet[bit3][mLastFound] = mDet[bitJ | bitI][j] * mDet[bit3][mLastFound] = mDet[bitJ | bitI][j] *
mDiffLength[k][mLastFound].dot(mPoints[j]) + mDiffLength[k][mLastFound].dot(mPoints[j]) +
mDet[bitJ | bitI][i] * mDet[bitJ | bitI][i] *
mDiffLength[k][mLastFound].dot(mPoints[i]); mDiffLength[k][mLastFound].dot(mPoints[i]);
} }
} }
} }
} }
if (mAllBits == 0xf) { if (mAllBits == 0xf) {
int k; int32_t k;
k = mNormSquare[1][0] < mNormSquare[2][0] ? k = mNormSquare[1][0] < mNormSquare[2][0] ?
(mNormSquare[1][0] < mNormSquare[3][0] ? 1 : 3) : (mNormSquare[1][0] < mNormSquare[3][0] ? 1 : 3) :
(mNormSquare[2][0] < mNormSquare[3][0] ? 2 : 3); (mNormSquare[2][0] < mNormSquare[3][0] ? 2 : 3);
mDet[0xf][0] = mDet[0xe][1] * mDiffLength[k][0].dot(mPoints[1]) + mDet[0xf][0] = mDet[0xe][1] * mDiffLength[k][0].dot(mPoints[1]) +
mDet[0xe][2] * mDiffLength[k][0].dot(mPoints[2]) + mDet[0xe][2] * mDiffLength[k][0].dot(mPoints[2]) +
mDet[0xe][3] * mDiffLength[k][0].dot(mPoints[3]); mDet[0xe][3] * mDiffLength[k][0].dot(mPoints[3]);
k = mNormSquare[0][1] < mNormSquare[2][1] ? k = mNormSquare[0][1] < mNormSquare[2][1] ?
(mNormSquare[0][1] < mNormSquare[3][1] ? 0 : 3) : (mNormSquare[0][1] < mNormSquare[3][1] ? 0 : 3) :
(mNormSquare[2][1] < mNormSquare[3][1] ? 2 : 3); (mNormSquare[2][1] < mNormSquare[3][1] ? 2 : 3);
mDet[0xf][1] = mDet[0xd][0] * mDiffLength[k][1].dot(mPoints[0]) + mDet[0xf][1] = mDet[0xd][0] * mDiffLength[k][1].dot(mPoints[0]) +
mDet[0xd][2] * mDiffLength[k][1].dot(mPoints[2]) + mDet[0xd][2] * mDiffLength[k][1].dot(mPoints[2]) +
mDet[0xd][3] * mDiffLength[k][1].dot(mPoints[3]); mDet[0xd][3] * mDiffLength[k][1].dot(mPoints[3]);
k = mNormSquare[0][2] < mNormSquare[1][2] ? k = mNormSquare[0][2] < mNormSquare[1][2] ?
(mNormSquare[0][2] < mNormSquare[3][2] ? 0 : 3) : (mNormSquare[0][2] < mNormSquare[3][2] ? 0 : 3) :
(mNormSquare[1][2] < mNormSquare[3][2] ? 1 : 3); (mNormSquare[1][2] < mNormSquare[3][2] ? 1 : 3);
mDet[0xf][2] = mDet[0xb][0] * mDiffLength[k][2].dot(mPoints[0]) + mDet[0xf][2] = mDet[0xb][0] * mDiffLength[k][2].dot(mPoints[0]) +
mDet[0xb][1] * mDiffLength[k][2].dot(mPoints[1]) + mDet[0xb][1] * mDiffLength[k][2].dot(mPoints[1]) +
mDet[0xb][3] * mDiffLength[k][2].dot(mPoints[3]); mDet[0xb][3] * mDiffLength[k][2].dot(mPoints[3]);
k = mNormSquare[0][3] < mNormSquare[1][3] ? k = mNormSquare[0][3] < mNormSquare[1][3] ?
(mNormSquare[0][3] < mNormSquare[2][3] ? 0 : 2) : (mNormSquare[0][3] < mNormSquare[2][3] ? 0 : 2) :
(mNormSquare[1][3] < mNormSquare[2][3] ? 1 : 2); (mNormSquare[1][3] < mNormSquare[2][3] ? 1 : 2);
mDet[0xf][3] = mDet[0x7][0] * mDiffLength[k][3].dot(mPoints[0]) + mDet[0xf][3] = mDet[0x7][0] * mDiffLength[k][3].dot(mPoints[0]) +
mDet[0x7][1] * mDiffLength[k][3].dot(mPoints[1]) + mDet[0x7][1] * mDiffLength[k][3].dot(mPoints[1]) +
mDet[0x7][2] * mDiffLength[k][3].dot(mPoints[2]); mDet[0x7][2] * mDiffLength[k][3].dot(mPoints[2]);
} }
} }
} }
// Return true if the subset is a proper subset. // 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 /// A proper subset X is a subset where for all point "y_i" in X, we have
/// detX_i value bigger than zero /// detX_i value bigger than zero
bool Simplex::isProperSubset(Bits subset) const { bool Simplex::isProperSubset(Bits subset) const {
int i; int32_t i;
Bits bit; Bits bit;
// For each four point of the possible simplex set // For each four point of the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<=1) { for (i=0, bit=0x1; i<4; i++, bit <<=1) {
if (overlap(subset, bit) && mDet[subset][i] <= 0.0) { if (overlap(subset, bit) && mDet[subset][i] <= 0.0) {
return false; return false;
} }
} }
return true; return true;
} }
// Return true if the set is affinely dependent. // Return true if the set is affinely dependent.
/// A set if affinely dependent if a point of the set /// A set if affinely dependent if a point of the set
/// is an affine combination of other points in the set /// is an affine combination of other points in the set
bool Simplex::isAffinelyDependent() const { bool Simplex::isAffinelyDependent() const {
decimal sum = 0.0; float sum = 0.0;
int i; int32_t i;
Bits bit; Bits bit;
// For each four point of the possible simplex set // For each four point of the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) { for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
if (overlap(mAllBits, bit)) { if (overlap(mAllBits, bit)) {
sum += mDet[mAllBits][i]; sum += mDet[mAllBits][i];
} }
} }
return (sum <= 0.0); return (sum <= 0.0);
} }
// Return true if the subset is a valid one for the closest point computation. // Return true if the subset is a valid one for the closest point computation.
/// A subset X is valid if : /// A subset X is valid if :
/// 1. delta(X)_i > 0 for each "i" in I_x and /// 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_ /// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_
bool Simplex::isValidSubset(Bits subset) const { bool Simplex::isValidSubset(Bits subset) const {
int i; int32_t i;
Bits bit; Bits bit;
// For each four point in the possible simplex set // For each four point in the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) { for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
if (overlap(mAllBits, bit)) { if (overlap(mAllBits, bit)) {
// If the current point is in the subset // If the current point is in the subset
if (overlap(subset, bit)) { if (overlap(subset, bit)) {
// If one delta(X)_i is smaller or equal to zero // If one delta(X)_i is smaller or equal to zero
if (mDet[subset][i] <= 0.0) { if (mDet[subset][i] <= 0.0) {
// The subset is not valid // The subset is not valid
return false; return false;
} }
} }
// If the point is not in the subset and the value delta(X U {y_j})_j // If the point is not in the subset and the value delta(X U {y_j})_j
// is bigger than zero // is bigger than zero
else if (mDet[subset | bit][i] > 0.0) { else if (mDet[subset | bit][i] > 0.0) {
// The subset is not valid // The subset is not valid
return false; return false;
} }
} }
} }
return true; return true;
} }
// Compute the closest points "pA" and "pB" of object A and B. // Compute the closest points "pA" and "pB" of object A and B.
/// The points are computed as follows : /// The points are computed as follows :
/// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A /// 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 /// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B
/// with lambda_i = deltaX_i / deltaX /// with lambda_i = deltaX_i / deltaX
void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
decimal deltaX = 0.0; float deltaX = 0.0;
pA.setAllValues(0.0, 0.0, 0.0); pA.setAllValues(0.0, 0.0, 0.0);
pB.setAllValues(0.0, 0.0, 0.0); pB.setAllValues(0.0, 0.0, 0.0);
int i; int32_t i;
Bits bit; Bits bit;
// For each four points in the possible simplex set // For each four points in the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) { for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
// If the current point is part of the simplex // If the current point is part of the simplex
if (overlap(mBitsCurrentSimplex, bit)) { if (overlap(mBitsCurrentSimplex, bit)) {
deltaX += mDet[mBitsCurrentSimplex][i]; deltaX += mDet[mBitsCurrentSimplex][i];
pA += mDet[mBitsCurrentSimplex][i] * mSuppPointsA[i]; pA += mDet[mBitsCurrentSimplex][i] * mSuppPointsA[i];
pB += mDet[mBitsCurrentSimplex][i] * mSuppPointsB[i]; pB += mDet[mBitsCurrentSimplex][i] * mSuppPointsB[i];
} }
} }
assert(deltaX > 0.0); assert(deltaX > 0.0);
decimal factor = decimal(1.0) / deltaX; float factor = float(1.0) / deltaX;
pA *= factor; pA *= factor;
pB *= factor; pB *= factor;
} }
// Compute the closest point "v" to the origin of the current simplex. // Compute the closest point "v" to the origin of the current simplex.
@ -319,76 +300,76 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
/// "v" of simplex that is closest to the origin. The method returns true /// "v" of simplex that is closest to the origin. The method returns true
/// if a closest point has been found. /// if a closest point has been found.
bool Simplex::computeClosestPoint(Vector3& v) { bool Simplex::computeClosestPoint(Vector3& v) {
Bits subset; Bits subset;
// For each possible simplex set // For each possible simplex set
for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) { for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) {
// If the simplex is a subset of the current simplex and is valid for the Johnson's // If the simplex is a subset of the current simplex and is valid for the Johnson's
// algorithm test // algorithm test
if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) { if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) {
mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex
v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex
return true; return true;
} }
} }
// If the simplex that contains only the last added point is valid for the Johnson's algorithm test // If the simplex that contains only the last added point is valid for the Johnson's algorithm test
if (isValidSubset(mLastFoundBit)) { if (isValidSubset(mLastFoundBit)) {
mBitsCurrentSimplex = mLastFoundBit; // Set the current simplex to the set that contains only the last added point mBitsCurrentSimplex = mLastFoundBit; // Set the current simplex to the set that contains only the last added point
mMaxLengthSquare = mPointsLengthSquare[mLastFound]; // Update the maximum square length mMaxLengthSquare = mPointsLengthSquare[mLastFound]; // Update the maximum square length
v = mPoints[mLastFound]; // The closest point of the simplex "v" is the last added point v = mPoints[mLastFound]; // The closest point of the simplex "v" is the last added point
return true; return true;
} }
// The algorithm failed to found a point // The algorithm failed to found a point
return false; return false;
} }
// Backup the closest point // Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3& v) { void Simplex::backupClosestPointInSimplex(Vector3& v) {
decimal minDistSquare = DECIMAL_LARGEST; float minDistSquare = DECIMAL_LARGEST;
Bits bit; Bits bit;
for (bit = mAllBits; bit != 0x0; bit--) { for (bit = mAllBits; bit != 0x0; bit--) {
if (isSubset(bit, mAllBits) && isProperSubset(bit)) { if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
Vector3 u = computeClosestPointForSubset(bit); Vector3 u = computeClosestPointForSubset(bit);
decimal distSquare = u.dot(u); float distSquare = u.dot(u);
if (distSquare < minDistSquare) { if (distSquare < minDistSquare) {
minDistSquare = distSquare; minDistSquare = distSquare;
mBitsCurrentSimplex = bit; mBitsCurrentSimplex = bit;
v = u; v = u;
} }
} }
} }
} }
// Return the closest point "v" in the convex hull of the points in the subset // Return the closest point "v" in the convex hull of the points in the subset
// represented by the bits "subset" // represented by the bits "subset"
Vector3 Simplex::computeClosestPointForSubset(Bits subset) { Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
mMaxLengthSquare = 0.0; mMaxLengthSquare = 0.0;
decimal deltaX = 0.0; // deltaX = sum of all det[subset][i] float deltaX = 0.0; // deltaX = sum of all det[subset][i]
int i; int32_t i;
Bits bit; Bits bit;
// For each four point in the possible simplex set // For each four point in the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) { for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
// If the current point is in the subset // If the current point is in the subset
if (overlap(subset, bit)) { if (overlap(subset, bit)) {
// deltaX = sum of all det[subset][i] // deltaX = sum of all det[subset][i]
deltaX += mDet[subset][i]; deltaX += mDet[subset][i];
if (mMaxLengthSquare < mPointsLengthSquare[i]) { if (mMaxLengthSquare < mPointsLengthSquare[i]) {
mMaxLengthSquare = mPointsLengthSquare[i]; mMaxLengthSquare = mPointsLengthSquare[i];
} }
// Closest point v = sum(lambda_i * points[i]) // Closest point v = sum(lambda_i * points[i])
v += mDet[subset][i] * mPoints[i]; v += mDet[subset][i] * mPoints[i];
} }
} }
assert(deltaX > 0.0); assert(deltaX > 0.0);
// Return the closet point "v" in the convex hull for the given subset // Return the closet point "v" in the convex hull for the given subset
return (decimal(1.0) / deltaX) * v; return (float(1.0) / deltaX) * v;
} }

194
ephysics/collision/narrowphase/GJK/Simplex.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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 // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -34,7 +13,7 @@
namespace reactphysics3d { namespace reactphysics3d {
// Type definitions // Type definitions
typedef unsigned int Bits; typedef uint32_t Bits;
// Class Simplex // Class Simplex
/** /**
@ -47,143 +26,142 @@ typedef unsigned int Bits;
*/ */
class Simplex { class Simplex {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Current points /// Current points
Vector3 mPoints[4]; Vector3 mPoints[4];
/// pointsLengthSquare[i] = (points[i].length)^2 /// pointsLengthSquare[i] = (points[i].length)^2
decimal mPointsLengthSquare[4]; float mPointsLengthSquare[4];
/// Maximum length of pointsLengthSquare[i] /// Maximum length of pointsLengthSquare[i]
decimal mMaxLengthSquare; float mMaxLengthSquare;
/// Support points of object A in local coordinates /// Support points of object A in local coordinates
Vector3 mSuppPointsA[4]; Vector3 mSuppPointsA[4];
/// Support points of object B in local coordinates /// Support points of object B in local coordinates
Vector3 mSuppPointsB[4]; Vector3 mSuppPointsB[4];
/// diff[i][j] contains points[i] - points[j] /// diff[i][j] contains points[i] - points[j]
Vector3 mDiffLength[4][4]; Vector3 mDiffLength[4][4];
/// Cached determinant values /// Cached determinant values
decimal mDet[16][4]; float mDet[16][4];
/// norm[i][j] = (diff[i][j].length())^2 /// norm[i][j] = (diff[i][j].length())^2
decimal mNormSquare[4][4]; float mNormSquare[4][4];
/// 4 bits that identify the current points of the simplex /// 4 bits that identify the current points of the simplex
/// For instance, 0101 means that points[1] and points[3] are in the simplex /// For instance, 0101 means that points[1] and points[3] are in the simplex
Bits mBitsCurrentSimplex; Bits mBitsCurrentSimplex;
/// Number between 1 and 4 that identify the last found support point /// Number between 1 and 4 that identify the last found support point
Bits mLastFound; Bits mLastFound;
/// Position of the last found support point (lastFoundBit = 0x1 << lastFound) /// Position of the last found support point (lastFoundBit = 0x1 << lastFound)
Bits mLastFoundBit; Bits mLastFoundBit;
/// allBits = bitsCurrentSimplex | lastFoundBit; /// allBits = bitsCurrentSimplex | lastFoundBit;
Bits mAllBits; Bits mAllBits;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
Simplex(const Simplex& simplex); Simplex(const Simplex& simplex);
/// Private assignment operator /// Private assignment operator
Simplex& operator=(const Simplex& simplex); Simplex& operator=(const Simplex& simplex);
/// Return true if some bits of "a" overlap with bits of "b" /// Return true if some bits of "a" overlap with bits of "b"
bool overlap(Bits a, Bits b) const; bool overlap(Bits a, Bits b) const;
/// Return true if the bits of "b" is a subset of the bits of "a" /// Return true if the bits of "b" is a subset of the bits of "a"
bool isSubset(Bits a, Bits b) const; bool isSubset(Bits a, Bits b) const;
/// Return true if the subset is a valid one for the closest point computation. /// Return true if the subset is a valid one for the closest point computation.
bool isValidSubset(Bits subset) const; bool isValidSubset(Bits subset) const;
/// Return true if the subset is a proper subset. /// Return true if the subset is a proper subset.
bool isProperSubset(Bits subset) const; bool isProperSubset(Bits subset) const;
/// Update the cached values used during the GJK algorithm /// Update the cached values used during the GJK algorithm
void updateCache(); void updateCache();
/// Compute the cached determinant values /// Compute the cached determinant values
void computeDeterminants(); void computeDeterminants();
/// Return the closest point "v" in the convex hull of a subset of points /// Return the closest point "v" in the convex hull of a subset of points
Vector3 computeClosestPointForSubset(Bits subset); Vector3 computeClosestPointForSubset(Bits subset);
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Simplex(); Simplex();
/// Destructor /// Destructor
~Simplex(); ~Simplex();
/// Return true if the simplex contains 4 points /// Return true if the simplex contains 4 points
bool isFull() const; bool isFull() const;
/// Return true if the simplex is empty /// Return true if the simplex is empty
bool isEmpty() const; bool isEmpty() const;
/// Return the points of the simplex /// Return the points of the simplex
unsigned int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB, uint32_t getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB,
Vector3* mPoints) const; Vector3* mPoints) const;
/// Return the maximum squared length of a point /// Return the maximum squared length of a point
decimal getMaxLengthSquareOfAPoint() const; float getMaxLengthSquareOfAPoint() const;
/// Add a new support point of (A-B) into the simplex. /// Add a new support point of (A-B) int32_to the simplex.
void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB);
/// Return true if the point is in the simplex /// Return true if the point is in the simplex
bool isPointInSimplex(const Vector3& point) const; bool isPointInSimplex(const Vector3& point) const;
/// Return true if the set is affinely dependent /// Return true if the set is affinely dependent
bool isAffinelyDependent() const; bool isAffinelyDependent() const;
/// Backup the closest point /// Backup the closest point
void backupClosestPointInSimplex(Vector3& point); void backupClosestPointInSimplex(Vector3& point);
/// Compute the closest points "pA" and "pB" of object A and B. /// Compute the closest points "pA" and "pB" of object A and B.
void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const; void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const;
/// Compute the closest point to the origin of the current simplex. /// Compute the closest point to the origin of the current simplex.
bool computeClosestPoint(Vector3& v); bool computeClosestPoint(Vector3& v);
}; };
// Return true if some bits of "a" overlap with bits of "b" // Return true if some bits of "a" overlap with bits of "b"
inline bool Simplex::overlap(Bits a, Bits b) const { inline bool Simplex::overlap(Bits a, Bits b) const {
return ((a & b) != 0x0); return ((a & b) != 0x0);
} }
// Return true if the bits of "b" is a subset of the bits of "a" // Return true if the bits of "b" is a subset of the bits of "a"
inline bool Simplex::isSubset(Bits a, Bits b) const { inline bool Simplex::isSubset(Bits a, Bits b) const {
return ((a & b) == a); return ((a & b) == a);
} }
// Return true if the simplex contains 4 points // Return true if the simplex contains 4 points
inline bool Simplex::isFull() const { inline bool Simplex::isFull() const {
return (mBitsCurrentSimplex == 0xf); return (mBitsCurrentSimplex == 0xf);
} }
// Return true if the simple is empty // Return true if the simple is empty
inline bool Simplex::isEmpty() const { inline bool Simplex::isEmpty() const {
return (mBitsCurrentSimplex == 0x0); return (mBitsCurrentSimplex == 0x0);
} }
// Return the maximum squared length of a point // Return the maximum squared length of a point
inline decimal Simplex::getMaxLengthSquareOfAPoint() const { inline float Simplex::getMaxLengthSquareOfAPoint() const {
return mMaxLengthSquare; return mMaxLengthSquare;
} }
} }
#endif

35
ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -31,7 +12,7 @@ using namespace reactphysics3d;
// Constructor // Constructor
NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() NarrowPhaseAlgorithm::NarrowPhaseAlgorithm()
: mMemoryAllocator(NULL), mCurrentOverlappingPair(NULL) { : mMemoryAllocator(NULL), mCurrentOverlappingPair(NULL) {
} }
@ -42,6 +23,6 @@ NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() {
// Initalize the algorithm // Initalize the algorithm
void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) { void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) {
mCollisionDetection = collisionDetection; mCollisionDetection = collisionDetection;
mMemoryAllocator = memoryAllocator; mMemoryAllocator = memoryAllocator;
} }

107
ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_ALGORITHM_H
#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
// Libraries // Libraries
#include <ephysics/body/Body.h> #include <ephysics/body/Body.h>
@ -45,12 +24,12 @@ class CollisionDetection;
*/ */
class NarrowPhaseCallback { class NarrowPhaseCallback {
public: public:
virtual ~NarrowPhaseCallback() = default; virtual ~NarrowPhaseCallback() = default;
/// Called by a narrow-phase collision algorithm when a new contact has been found /// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo)=0; const ContactPointInfo& contactInfo)=0;
}; };
@ -62,56 +41,54 @@ class NarrowPhaseCallback {
*/ */
class NarrowPhaseAlgorithm { class NarrowPhaseAlgorithm {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the collision detection object /// Pointer to the collision detection object
CollisionDetection* mCollisionDetection; CollisionDetection* mCollisionDetection;
/// Pointer to the memory allocator /// Pointer to the memory allocator
MemoryAllocator* mMemoryAllocator; MemoryAllocator* mMemoryAllocator;
/// Overlapping pair of the bodies currently tested for collision /// Overlapping pair of the bodies currently tested for collision
OverlappingPair* mCurrentOverlappingPair; OverlappingPair* mCurrentOverlappingPair;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm); NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm);
/// Private assignment operator /// Private assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm); NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
NarrowPhaseAlgorithm(); NarrowPhaseAlgorithm();
/// Destructor /// Destructor
virtual ~NarrowPhaseAlgorithm(); virtual ~NarrowPhaseAlgorithm();
/// Initalize the algorithm /// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator); virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator);
/// Set the current overlapping pair of bodies /// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(OverlappingPair* overlappingPair); void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
/// Compute a contact info if the two bounding volume collide /// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info, virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback)=0; NarrowPhaseCallback* narrowPhaseCallback)=0;
}; };
// Set the current overlapping pair of bodies // Set the current overlapping pair of bodies
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) { inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
mCurrentOverlappingPair = overlappingPair; mCurrentOverlappingPair = overlappingPair;
} }
} }
#endif

100
ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h> #include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h>
@ -31,50 +12,45 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() : NarrowPhaseAlgorithm() { SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() :
NarrowPhaseAlgorithm() {
} }
// Destructor // Destructor
SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() { SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
} }
void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
// Get the sphere collision shapes
// Get the sphere collision shapes const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape); const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape); // Get the local-space to world-space transforms
const Transform& transform1 = shape1Info.shapeToWorldTransform;
// Get the local-space to world-space transforms const Transform& transform2 = shape2Info.shapeToWorldTransform;
const Transform& transform1 = shape1Info.shapeToWorldTransform; // Compute the distance between the centers
const Transform& transform2 = shape2Info.shapeToWorldTransform; Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
float squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the distance between the centers // Compute the sum of the radius
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); // If the sphere collision shapes int32_tersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
// Compute the sum of the radius Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 int32_tersectionOnBody1 = sphereShape1->getRadius() *
// If the sphere collision shapes intersect centerSphere2InBody1LocalSpace.getUnit();
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { Vector3 int32_tersectionOnBody2 = sphereShape2->getRadius() *
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); centerSphere1InBody2LocalSpace.getUnit();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); float penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit(); // Create the contact info object
Vector3 intersectionOnBody2 = sphereShape2->getRadius() * ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
centerSphere1InBody2LocalSpace.getUnit(); shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); int32_tersectionOnBody1, int32_tersectionOnBody2);
// Notify about the new contact
// Create the contact info object narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
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);
}
} }

68
ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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 // Libraries
#include <ephysics/body/Body.h> #include <ephysics/body/Body.h>
@ -42,33 +21,32 @@ namespace reactphysics3d {
*/ */
class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
protected : protected :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm); SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm);
/// Private assignment operator /// Private assignment operator
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm); SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
SphereVsSphereAlgorithm(); SphereVsSphereAlgorithm();
/// Destructor /// Destructor
virtual ~SphereVsSphereAlgorithm(); virtual ~SphereVsSphereAlgorithm();
/// Compute a contact info if the two bounding volume collide /// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info, virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
}; };
} }
#endif

154
ephysics/collision/shapes/AABB.cpp
View File

@ -1,27 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/AABB.h> #include <ephysics/collision/shapes/AABB.h>
@ -38,13 +20,13 @@ AABB::AABB() {
// Constructor // Constructor
AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates) AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
:mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) { :mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) {
} }
// Copy-constructor // Copy-constructor
AABB::AABB(const AABB& aabb) AABB::AABB(const AABB& aabb)
: mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) { : mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) {
} }
@ -55,96 +37,96 @@ AABB::~AABB() {
// Merge the AABB in parameter with the current one // Merge the AABB in parameter with the current one
void AABB::mergeWithAABB(const AABB& aabb) { void AABB::mergeWithAABB(const AABB& aabb) {
mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x); mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x);
mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y); mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y);
mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z); mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z);
mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x); mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x);
mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y); mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y);
mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z); mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z);
} }
// Replace the current AABB with a new AABB that is the union of two AABBs in parameters // Replace the current AABB with a new AABB that is the union of two AABBs in parameters
void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) { void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x); mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x);
mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y); mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y);
mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z); mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z);
mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x); mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x);
mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y); mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y);
mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z); mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z);
} }
// Return true if the current AABB contains the AABB given in parameter // Return true if the current AABB contains the AABB given in parameter
bool AABB::contains(const AABB& aabb) const { bool AABB::contains(const AABB& aabb) const {
bool isInside = true; bool isInside = true;
isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x; isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;
isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y; isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y;
isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z; isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z;
isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x; isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x;
isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y; isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y;
isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z; isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z;
return isInside; return isInside;
} }
// Create and return an AABB for a triangle // Create and return an AABB for a triangle
AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) { AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x; if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x;
if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y; if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y;
if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z; if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z;
if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x; if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x;
if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y; if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y;
if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z; if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z;
if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x; if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x;
if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y; if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y;
if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z; if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z;
if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x; if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x;
if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y; if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y;
if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z; if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z;
return AABB(minCoords, maxCoords); return AABB(minCoords, maxCoords);
} }
// Return true if the ray intersects the AABB // Return true if the ray int32_tersects the AABB
/// This method use the line vs AABB raycasting technique described in /// This method use the line vs AABB raycasting technique described in
/// Real-time Collision Detection by Christer Ericson. /// Real-time Collision Detection by Christer Ericson.
bool AABB::testRayIntersect(const Ray& ray) const { bool AABB::testRayIntersect(const Ray& ray) const {
const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const Vector3 e = mMaxCoordinates - mMinCoordinates; const Vector3 e = mMaxCoordinates - mMinCoordinates;
const Vector3 d = point2 - ray.point1; const Vector3 d = point2 - ray.point1;
const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates; const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates;
// Test if the AABB face normals are separating axis // Test if the AABB face normals are separating axis
decimal adx = std::abs(d.x); float adx = std::abs(d.x);
if (std::abs(m.x) > e.x + adx) return false; if (std::abs(m.x) > e.x + adx) return false;
decimal ady = std::abs(d.y); float ady = std::abs(d.y);
if (std::abs(m.y) > e.y + ady) return false; if (std::abs(m.y) > e.y + ady) return false;
decimal adz = std::abs(d.z); float adz = std::abs(d.z);
if (std::abs(m.z) > e.z + adz) return false; if (std::abs(m.z) > e.z + adz) return false;
// Add in an epsilon term to counteract arithmetic errors when segment is // Add in an epsilon term to counteract arithmetic errors when segment is
// (near) parallel to a coordinate axis (see text for detail) // (near) parallel to a coordinate axis (see text for detail)
const decimal epsilon = 0.00001; const float epsilon = 0.00001;
adx += epsilon; adx += epsilon;
ady += epsilon; ady += epsilon;
adz += epsilon; adz += epsilon;
// Test if the cross products between face normals and ray direction are // Test if the cross products between face normals and ray direction are
// separating axis // separating axis
if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false; if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false;
if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false; if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false;
if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false; if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false;
// No separating axis has been found // No separating axis has been found
return true; return true;
} }

209
ephysics/collision/shapes/AABB.h
View File

@ -1,37 +1,16 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_AABB_H
#define REACTPHYSICS3D_AABB_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
/// ReactPhysics3D namespace /// ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
// Class AABB // Class AABB
/** /**
* This class represents a bounding volume of type "Axis Aligned * This class represents a bounding volume of type "Axis Aligned
@ -41,111 +20,111 @@ namespace reactphysics3d {
*/ */
class AABB { class AABB {
private : private :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Minimum world coordinates of the AABB on the x,y and z axis /// Minimum world coordinates of the AABB on the x,y and z axis
Vector3 mMinCoordinates; Vector3 mMinCoordinates;
/// Maximum world coordinates of the AABB on the x,y and z axis /// Maximum world coordinates of the AABB on the x,y and z axis
Vector3 mMaxCoordinates; Vector3 mMaxCoordinates;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
AABB(); AABB();
/// Constructor /// Constructor
AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates); AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
/// Copy-constructor /// Copy-constructor
AABB(const AABB& aabb); AABB(const AABB& aabb);
/// Destructor /// Destructor
~AABB(); ~AABB();
/// Return the center point /// Return the center point
Vector3 getCenter() const; Vector3 getCenter() const;
/// Return the minimum coordinates of the AABB /// Return the minimum coordinates of the AABB
const Vector3& getMin() const; const Vector3& getMin() const;
/// Set the minimum coordinates of the AABB /// Set the minimum coordinates of the AABB
void setMin(const Vector3& min); void setMin(const Vector3& min);
/// Return the maximum coordinates of the AABB /// Return the maximum coordinates of the AABB
const Vector3& getMax() const; const Vector3& getMax() const;
/// Set the maximum coordinates of the AABB /// Set the maximum coordinates of the AABB
void setMax(const Vector3& max); void setMax(const Vector3& max);
/// Return the size of the AABB in the three dimension x, y and z /// Return the size of the AABB in the three dimension x, y and z
Vector3 getExtent() const; Vector3 getExtent() const;
/// Inflate each side of the AABB by a given size /// Inflate each side of the AABB by a given size
void inflate(decimal dx, decimal dy, decimal dz); void inflate(float dx, float dy, float dz);
/// Return true if the current AABB is overlapping with the AABB in argument /// Return true if the current AABB is overlapping with the AABB in argument
bool testCollision(const AABB& aabb) const; bool testCollision(const AABB& aabb) const;
/// Return the volume of the AABB /// Return the volume of the AABB
decimal getVolume() const; float getVolume() const;
/// Merge the AABB in parameter with the current one /// Merge the AABB in parameter with the current one
void mergeWithAABB(const AABB& aabb); void mergeWithAABB(const AABB& aabb);
/// Replace the current AABB with a new AABB that is the union of two AABBs in parameters /// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2); void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);
/// Return true if the current AABB contains the AABB given in parameter /// Return true if the current AABB contains the AABB given in parameter
bool contains(const AABB& aabb) const; bool contains(const AABB& aabb) const;
/// Return true if a point is inside the AABB /// Return true if a point is inside the AABB
bool contains(const Vector3& point) const; bool contains(const Vector3& point) const;
/// Return true if the AABB of a triangle intersects the AABB /// Return true if the AABB of a triangle int32_tersects the AABB
bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
/// Return true if the ray intersects the AABB /// Return true if the ray int32_tersects the AABB
bool testRayIntersect(const Ray& ray) const; bool testRayIntersect(const Ray& ray) const;
/// Create and return an AABB for a triangle /// Create and return an AABB for a triangle
static AABB createAABBForTriangle(const Vector3* trianglePoints); static AABB createAABBForTriangle(const Vector3* trianglePoints);
/// Assignment operator /// Assignment operator
AABB& operator=(const AABB& aabb); AABB& operator=(const AABB& aabb);
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicAABBTree; friend class DynamicAABBTree;
}; };
// Return the center point of the AABB in world coordinates // Return the center point of the AABB in world coordinates
inline Vector3 AABB::getCenter() const { inline Vector3 AABB::getCenter() const {
return (mMinCoordinates + mMaxCoordinates) * decimal(0.5); return (mMinCoordinates + mMaxCoordinates) * float(0.5);
} }
// Return the minimum coordinates of the AABB // Return the minimum coordinates of the AABB
inline const Vector3& AABB::getMin() const { inline const Vector3& AABB::getMin() const {
return mMinCoordinates; return mMinCoordinates;
} }
// Set the minimum coordinates of the AABB // Set the minimum coordinates of the AABB
inline void AABB::setMin(const Vector3& min) { inline void AABB::setMin(const Vector3& min) {
mMinCoordinates = min; mMinCoordinates = min;
} }
// Return the maximum coordinates of the AABB // Return the maximum coordinates of the AABB
inline const Vector3& AABB::getMax() const { inline const Vector3& AABB::getMax() const {
return mMaxCoordinates; return mMaxCoordinates;
} }
// Set the maximum coordinates of the AABB // Set the maximum coordinates of the AABB
inline void AABB::setMax(const Vector3& max) { inline void AABB::setMax(const Vector3& max) {
mMaxCoordinates = max; mMaxCoordinates = max;
} }
// Return the size of the AABB in the three dimension x, y and z // Return the size of the AABB in the three dimension x, y and z
@ -154,60 +133,58 @@ inline Vector3 AABB::getExtent() const {
} }
// Inflate each side of the AABB by a given size // Inflate each side of the AABB by a given size
inline void AABB::inflate(decimal dx, decimal dy, decimal dz) { inline void AABB::inflate(float dx, float dy, float dz) {
mMaxCoordinates += Vector3(dx, dy, dz); mMaxCoordinates += Vector3(dx, dy, dz);
mMinCoordinates -= Vector3(dx, dy, dz); mMinCoordinates -= Vector3(dx, dy, dz);
} }
// Return true if the current AABB is overlapping with the AABB in argument. // Return true if the current AABB is overlapping with the AABB in argument.
/// Two AABBs overlap if they overlap in the three x, y and z axis at the same time /// Two AABBs overlap if they overlap in the three x, y and z axis at the same time
inline bool AABB::testCollision(const AABB& aabb) const { inline bool AABB::testCollision(const AABB& aabb) const {
if (mMaxCoordinates.x < aabb.mMinCoordinates.x || if (mMaxCoordinates.x < aabb.mMinCoordinates.x ||
aabb.mMaxCoordinates.x < mMinCoordinates.x) return false; aabb.mMaxCoordinates.x < mMinCoordinates.x) return false;
if (mMaxCoordinates.y < aabb.mMinCoordinates.y || if (mMaxCoordinates.y < aabb.mMinCoordinates.y ||
aabb.mMaxCoordinates.y < mMinCoordinates.y) return false; aabb.mMaxCoordinates.y < mMinCoordinates.y) return false;
if (mMaxCoordinates.z < aabb.mMinCoordinates.z|| if (mMaxCoordinates.z < aabb.mMinCoordinates.z||
aabb.mMaxCoordinates.z < mMinCoordinates.z) return false; aabb.mMaxCoordinates.z < mMinCoordinates.z) return false;
return true; return true;
} }
// Return the volume of the AABB // Return the volume of the AABB
inline decimal AABB::getVolume() const { inline float AABB::getVolume() const {
const Vector3 diff = mMaxCoordinates - mMinCoordinates; const Vector3 diff = mMaxCoordinates - mMinCoordinates;
return (diff.x * diff.y * diff.z); return (diff.x * diff.y * diff.z);
} }
// Return true if the AABB of a triangle intersects the AABB // Return true if the AABB of a triangle int32_tersects the AABB
inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const { inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const {
if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false; if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false;
if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false; if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false;
if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false; if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false;
if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false; if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false;
if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false; if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false;
if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false; if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false;
return true; return true;
} }
// Return true if a point is inside the AABB // Return true if a point is inside the AABB
inline bool AABB::contains(const Vector3& point) const { inline bool AABB::contains(const Vector3& point) const {
return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON && return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON &&
point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON && point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON &&
point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON); point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON);
} }
// Assignment operator // Assignment operator
inline AABB& AABB::operator=(const AABB& aabb) { inline AABB& AABB::operator=(const AABB& aabb) {
if (this != &aabb) { if (this != &aabb) {
mMinCoordinates = aabb.mMinCoordinates; mMinCoordinates = aabb.mMinCoordinates;
mMaxCoordinates = aabb.mMaxCoordinates; mMaxCoordinates = aabb.mMaxCoordinates;
} }
return *this; return *this;
} }
} }
#endif

155
ephysics/collision/shapes/BoxShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2015 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/BoxShape.h> #include <ephysics/collision/shapes/BoxShape.h>
@ -37,11 +18,11 @@ using namespace reactphysics3d;
* @param extent The vector with the three extents of the box (in meters) * @param extent The vector with the three extents of the box (in meters)
* @param margin The collision margin (in meters) around the collision shape * @param margin The collision margin (in meters) around the collision shape
*/ */
BoxShape::BoxShape(const Vector3& extent, decimal margin) BoxShape::BoxShape(const Vector3& extent, float margin)
: ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { : ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > decimal(0.0) && extent.x > margin); assert(extent.x > float(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin); assert(extent.y > float(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin); assert(extent.z > float(0.0) && extent.z > margin);
} }
// Destructor // Destructor
@ -52,81 +33,81 @@ BoxShape::~BoxShape() {
// Return the local inertia tensor of the collision shape // Return the local inertia tensor of the collision shape
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass; float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin); Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
decimal xSquare = realExtent.x * realExtent.x; float xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y; float ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z; float zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0, 0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare)); 0.0, 0.0, factor * (xSquare + ySquare));
} }
// Raycast method with feedback information // 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) const {
Vector3 rayDirection = ray.point2 - ray.point1; Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST; float tMin = DECIMAL_SMALLEST;
decimal tMax = DECIMAL_LARGEST; float tMax = DECIMAL_LARGEST;
Vector3 normalDirection(decimal(0), decimal(0), decimal(0)); Vector3 normalDirection(float(0), float(0), float(0));
Vector3 currentNormal; Vector3 currentNormal;
// For each of the three slabs // For each of the three slabs
for (int i=0; i<3; i++) { for (int32_t i=0; i<3; i++) {
// If ray is parallel to the slab // If ray is parallel to the slab
if (std::abs(rayDirection[i]) < MACHINE_EPSILON) { if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
// If the ray's origin is not inside the slab, there is no hit // If the ray's origin is not inside the slab, there is no hit
if (ray.point1[i] > mExtent[i] || ray.point1[i] < -mExtent[i]) return false; if (ray.point1[i] > mExtent[i] || ray.point1[i] < -mExtent[i]) return false;
} }
else { else {
// Compute the intersection of the ray with the near and far plane of the slab // Compute the int32_tersection of the ray with the near and far plane of the slab
decimal oneOverD = decimal(1.0) / rayDirection[i]; float oneOverD = float(1.0) / rayDirection[i];
decimal t1 = (-mExtent[i] - ray.point1[i]) * oneOverD; float t1 = (-mExtent[i] - ray.point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - ray.point1[i]) * oneOverD; float t2 = (mExtent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0); currentNormal[0] = (i == 0) ? -mExtent[i] : float(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0); currentNormal[1] = (i == 1) ? -mExtent[i] : float(0.0);
currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0); currentNormal[2] = (i == 2) ? -mExtent[i] : float(0.0);
// Swap t1 and t2 if need so that t1 is intersection with near plane and // Swap t1 and t2 if need so that t1 is int32_tersection with near plane and
// t2 with far plane // t2 with far plane
if (t1 > t2) { if (t1 > t2) {
std::swap(t1, t2); std::swap(t1, t2);
currentNormal = -currentNormal; currentNormal = -currentNormal;
} }
// Compute the intersection of the of slab intersection interval with previous slabs // Compute the int32_tersection of the of slab int32_tersection int32_terval with previous slabs
if (t1 > tMin) { if (t1 > tMin) {
tMin = t1; tMin = t1;
normalDirection = currentNormal; normalDirection = currentNormal;
} }
tMax = std::min(tMax, t2); tMax = std::min(tMax, t2);
// If tMin is larger than the maximum raycasting fraction, we return no hit // If tMin is larger than the maximum raycasting fraction, we return no hit
if (tMin > ray.maxFraction) return false; if (tMin > ray.maxFraction) return false;
// If the slabs intersection is empty, there is no hit // If the slabs int32_tersection is empty, there is no hit
if (tMin > tMax) return false; if (tMin > tMax) return false;
} }
} }
// If tMin is negative, we return no hit // If tMin is negative, we return no hit
if (tMin < decimal(0.0) || tMin > ray.maxFraction) return false; if (tMin < float(0.0) || tMin > ray.maxFraction) return false;
// The ray intersects the three slabs, we compute the hit point // The ray int32_tersects the three slabs, we compute the hit point
Vector3 localHitPoint = ray.point1 + tMin * rayDirection; Vector3 localHitPoint = ray.point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = tMin; raycastInfo.hitFraction = tMin;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }

129
ephysics/collision/shapes/BoxShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_BOX_SHAPE_H
#define REACTPHYSICS3D_BOX_SHAPE_H
// Libraries // Libraries
#include <cfloat> #include <cfloat>
@ -52,55 +31,55 @@ namespace reactphysics3d {
*/ */
class BoxShape : public ConvexShape { class BoxShape : public ConvexShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Extent sizes of the box in the x, y and z direction /// Extent sizes of the box in the x, y and z direction
Vector3 mExtent; Vector3 mExtent;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
BoxShape(const BoxShape& shape); BoxShape(const BoxShape& shape);
/// Private assignment operator /// Private assignment operator
BoxShape& operator=(const BoxShape& shape); BoxShape& operator=(const BoxShape& shape);
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// 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;
/// Raycast method with feedback information /// 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) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN); BoxShape(const Vector3& extent, float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~BoxShape(); virtual ~BoxShape();
/// Return the extents of the box /// Return the extents of the box
Vector3 getExtent() const; Vector3 getExtent() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions /// 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;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
}; };
// Return the extents of the box // Return the extents of the box
@ -108,15 +87,15 @@ class BoxShape : public ConvexShape {
* @return The vector with the three extents of the box shape (in meters) * @return The vector with the three extents of the box shape (in meters)
*/ */
inline Vector3 BoxShape::getExtent() const { inline Vector3 BoxShape::getExtent() const {
return mExtent + Vector3(mMargin, mMargin, mMargin); return mExtent + Vector3(mMargin, mMargin, mMargin);
} }
// Set the scaling vector of the collision shape // Set the scaling vector of the collision shape
inline void BoxShape::setLocalScaling(const Vector3& scaling) { inline void BoxShape::setLocalScaling(const Vector3& scaling) {
mExtent = (mExtent / mScaling) * scaling; mExtent = (mExtent / mScaling) * scaling;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
// Return the local bounds of the shape in x, y and z directions // Return the local bounds of the shape in x, y and z directions
@ -127,34 +106,32 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) {
*/ */
inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds // Maximum bounds
max = mExtent + Vector3(mMargin, mMargin, mMargin); max = mExtent + Vector3(mMargin, mMargin, mMargin);
// Minimum bounds // Minimum bounds
min = -max; min = -max;
} }
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t BoxShape::getSizeInBytes() const { inline size_t BoxShape::getSizeInBytes() const {
return sizeof(BoxShape); return sizeof(BoxShape);
} }
// Return a local support point in a given direction without the objec margin // Return a local support point in a given direction without the objec margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction, inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
direction.y < 0.0 ? -mExtent.y : mExtent.y, direction.y < 0.0 ? -mExtent.y : mExtent.y,
direction.z < 0.0 ? -mExtent.z : mExtent.z); direction.z < 0.0 ? -mExtent.z : mExtent.z);
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] && return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] &&
localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] && localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] &&
localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]); localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]);
} }
} }
#endif

371
ephysics/collision/shapes/CapsuleShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/CapsuleShape.h> #include <ephysics/collision/shapes/CapsuleShape.h>
@ -36,10 +17,10 @@ using namespace reactphysics3d;
* @param radius The radius of the capsule (in meters) * @param radius The radius of the capsule (in meters)
* @param height The height of the capsule (in meters) * @param height The height of the capsule (in meters)
*/ */
CapsuleShape::CapsuleShape(decimal radius, decimal height) CapsuleShape::CapsuleShape(float radius, float height)
: ConvexShape(CAPSULE, radius), mHalfHeight(height * decimal(0.5)) { : ConvexShape(CAPSULE, radius), mHalfHeight(height * float(0.5)) {
assert(radius > decimal(0.0)); assert(radius > float(0.0));
assert(height > decimal(0.0)); assert(height > float(0.0));
} }
// Destructor // Destructor
@ -50,232 +31,232 @@ CapsuleShape::~CapsuleShape() {
// Return the local inertia tensor of the capsule // Return the local inertia tensor of the capsule
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1 // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
decimal height = mHalfHeight + mHalfHeight; float height = mHalfHeight + mHalfHeight;
decimal radiusSquare = mMargin * mMargin; float radiusSquare = mMargin * mMargin;
decimal heightSquare = height * height; float heightSquare = height * height;
decimal radiusSquareDouble = radiusSquare + radiusSquare; float radiusSquareDouble = radiusSquare + radiusSquare;
decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height); float factor1 = float(2.0) * mMargin / (float(4.0) * mMargin + float(3.0) * height);
decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height); float factor2 = float(3.0) * height / (float(4.0) * mMargin + float(3.0) * height);
decimal sum1 = decimal(0.4) * radiusSquareDouble; float sum1 = float(0.4) * radiusSquareDouble;
decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare; float sum2 = float(0.75) * height * mMargin + float(0.5) * heightSquare;
decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare; float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare;
decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3; float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble; float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble;
tensor.setAllValues(IxxAndzz, 0.0, 0.0, tensor.setAllValues(IxxAndzz, 0.0, 0.0,
0.0, Iyy, 0.0, 0.0, Iyy, 0.0,
0.0, 0.0, IxxAndzz); 0.0, 0.0, IxxAndzz);
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal diffYCenterSphere1 = localPoint.y - mHalfHeight; const float diffYCenterSphere1 = localPoint.y - mHalfHeight;
const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight; const float diffYCenterSphere2 = localPoint.y + mHalfHeight;
const decimal xSquare = localPoint.x * localPoint.x; const float xSquare = localPoint.x * localPoint.x;
const decimal zSquare = localPoint.z * localPoint.z; const float zSquare = localPoint.z * localPoint.z;
const decimal squareRadius = mMargin * mMargin; const float squareRadius = mMargin * mMargin;
// Return true if the point is inside the cylinder or one of the two spheres of the capsule // Return true if the point is inside the cylinder or one of the two spheres of the capsule
return ((xSquare + zSquare) < squareRadius && return ((xSquare + zSquare) < squareRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) || localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) ||
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
} }
// Raycast method with feedback information // 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) const {
const Vector3 n = ray.point2 - ray.point1; const Vector3 n = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.01); const float epsilon = float(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0)); Vector3 p(float(0), -mHalfHeight, float(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0)); Vector3 q(float(0), mHalfHeight, float(0));
Vector3 d = q - p; Vector3 d = q - p;
Vector3 m = ray.point1 - p; Vector3 m = ray.point1 - p;
decimal t; float t;
decimal mDotD = m.dot(d); float mDotD = m.dot(d);
decimal nDotD = n.dot(d); float nDotD = n.dot(d);
decimal dDotD = d.dot(d); float dDotD = d.dot(d);
// Test if the segment is outside the cylinder // Test if the segment is outside the cylinder
decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mMargin, decimal(0.0))).dot(d); float vec1DotD = (ray.point1 - Vector3(float(0.0), -mHalfHeight - mMargin, float(0.0))).dot(d);
if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false; if (vec1DotD < float(0.0) && vec1DotD + nDotD < float(0.0)) return false;
decimal ddotDExtraCaps = decimal(2.0) * mMargin * d.y; float ddotDExtraCaps = float(2.0) * mMargin * d.y;
if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false; if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false;
decimal nDotN = n.dot(n); float nDotN = n.dot(n);
decimal mDotN = m.dot(n); float mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD; float a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mMargin * mMargin; float k = m.dot(m) - mMargin * mMargin;
decimal c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis // If the ray is parallel to the capsule axis
if (std::abs(a) < epsilon) { if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the capusle's cylinder, we return no hit // If the origin is outside the surface of the capusle's cylinder, we return no hit
if (c > decimal(0.0)) return false; if (c > float(0.0)) return false;
// Here we know that the segment intersect an endcap of the capsule // Here we know that the segment int32_tersect an endcap of the capsule
// If the ray intersects with the "p" endcap of the capsule // If the ray int32_tersects with the "p" endcap of the capsule
if (mDotD < decimal(0.0)) { if (mDotD < float(0.0)) {
// Check intersection between the ray and the "p" sphere endcap of the capsule // Check int32_tersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint; Vector3 hitLocalPoint;
decimal hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - p; Vector3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} }
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder
// Check intersection between the ray and the "q" sphere endcap of the capsule // Check int32_tersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint; Vector3 hitLocalPoint;
decimal hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - q; Vector3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} }
else { // If the origin is inside the cylinder, we return no hit else { // If the origin is inside the cylinder, we return no hit
return false; return false;
} }
} }
decimal b = dDotD * mDotN - nDotD * mDotD; float b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c; float discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit // If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < decimal(0.0)) return false; if (discriminant < float(0.0)) return false;
// Compute the smallest root (first intersection along the ray) // Compute the smallest root (first int32_tersection along the ray)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a; float t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the finite cylinder of the capsule on "p" endcap side // If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side
decimal value = mDotD + t * nDotD; float value = mDotD + t * nDotD;
if (value < decimal(0.0)) { if (value < float(0.0)) {
// Check intersection between the ray and the "p" sphere endcap of the capsule // Check int32_tersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint; Vector3 hitLocalPoint;
decimal hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - p; Vector3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} }
else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side else if (value > dDotD) { // If the int32_tersection is outside the finite cylinder on the "q" side
// Check intersection between the ray and the "q" sphere endcap of the capsule // Check int32_tersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint; Vector3 hitLocalPoint;
decimal hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - q; Vector3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} }
t = t0; t = t0;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p; Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d; Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
// Raycasting method between a ray one of the two spheres end cap of the capsule // Raycasting method between a ray one of the two spheres end cap of the capsule
bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction, const Vector3& sphereCenter, float maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const { Vector3& hitLocalPoint, float& hitFraction) const {
const Vector3 m = point1 - sphereCenter; const Vector3 m = point1 - sphereCenter;
decimal c = m.dot(m) - mMargin * mMargin; float c = m.dot(m) - mMargin * mMargin;
// If the origin of the ray is inside the sphere, we return no intersection // If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < decimal(0.0)) return false; if (c < float(0.0)) return false;
const Vector3 rayDirection = point2 - point1; const Vector3 rayDirection = point2 - point1;
decimal b = m.dot(rayDirection); float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray // If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no intersection // is pointing away from the sphere, there is no int32_tersection
if (b > decimal(0.0)) return false; if (b > float(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare(); float raySquareLength = rayDirection.lengthSquare();
// Compute the discriminant of the quadratic equation // Compute the discriminant of the quadratic equation
decimal discriminant = b * b - raySquareLength * c; float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no intersection // If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false; if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
decimal t = -b - std::sqrt(discriminant); float t = -b - std::sqrt(discriminant);
assert(t >= decimal(0.0)); assert(t >= float(0.0));
// If the hit point is withing the segment ray fraction // If the hit point is withing the segment ray fraction
if (t < maxFraction * raySquareLength) { if (t < maxFraction * raySquareLength) {
// Compute the intersection information // Compute the int32_tersection information
t /= raySquareLength; t /= raySquareLength;
hitFraction = t; hitFraction = t;
hitLocalPoint = point1 + t * rayDirection; hitLocalPoint = point1 + t * rayDirection;
return true; return true;
} }
return false; return false;
} }

167
ephysics/collision/shapes/CapsuleShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SHAPE_H
#define REACTPHYSICS3D_CAPSULE_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -46,93 +25,93 @@ namespace reactphysics3d {
*/ */
class CapsuleShape : public ConvexShape { class CapsuleShape : public ConvexShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Half height of the capsule (height = distance between the centers of the two spheres) /// Half height of the capsule (height = distance between the centers of the two spheres)
decimal mHalfHeight; float mHalfHeight;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
CapsuleShape(const CapsuleShape& shape); CapsuleShape(const CapsuleShape& shape);
/// Private assignment operator /// Private assignment operator
CapsuleShape& operator=(const CapsuleShape& shape); CapsuleShape& operator=(const CapsuleShape& shape);
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// 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;
/// Raycast method with feedback information /// 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) const;
/// Raycasting method between a ray one of the two spheres end cap of the capsule /// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction, const Vector3& sphereCenter, float maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const; Vector3& hitLocalPoint, float& hitFraction) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
CapsuleShape(decimal radius, decimal height); CapsuleShape(float radius, float height);
/// Destructor /// Destructor
virtual ~CapsuleShape(); virtual ~CapsuleShape();
/// Return the radius of the capsule /// Return the radius of the capsule
decimal getRadius() const; float getRadius() const;
/// Return the height of the capsule /// Return the height of the capsule
decimal getHeight() const; float getHeight() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions /// 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;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
}; };
// Get the radius of the capsule // Get the radius of the capsule
/** /**
* @return The radius of the capsule shape (in meters) * @return The radius of the capsule shape (in meters)
*/ */
inline decimal CapsuleShape::getRadius() const { inline float CapsuleShape::getRadius() const {
return mMargin; return mMargin;
} }
// Return the height of the capsule // Return the height of the capsule
/** /**
* @return The height of the capsule shape (in meters) * @return The height of the capsule shape (in meters)
*/ */
inline decimal CapsuleShape::getHeight() const { inline float CapsuleShape::getHeight() const {
return mHalfHeight + mHalfHeight; return mHalfHeight + mHalfHeight;
} }
// Set the scaling vector of the collision shape // Set the scaling vector of the collision shape
inline void CapsuleShape::setLocalScaling(const Vector3& scaling) { inline void CapsuleShape::setLocalScaling(const Vector3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mMargin = (mMargin / mScaling.x) * scaling.x; mMargin = (mMargin / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t CapsuleShape::getSizeInBytes() const { inline size_t CapsuleShape::getSizeInBytes() const {
return sizeof(CapsuleShape); return sizeof(CapsuleShape);
} }
// Return the local bounds of the shape in x, y and z directions // Return the local bounds of the shape in x, y and z directions
@ -143,15 +122,15 @@ inline size_t CapsuleShape::getSizeInBytes() const {
*/ */
inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds // Maximum bounds
max.x = mMargin; max.x = mMargin;
max.y = mHalfHeight + mMargin; max.y = mHalfHeight + mMargin;
max.z = mMargin; max.z = mMargin;
// Minimum bounds // Minimum bounds
min.x = -mMargin; min.x = -mMargin;
min.y = -max.y; min.y = -max.y;
min.z = min.x; min.z = min.x;
} }
// Return a local support point in a given direction without the object margin. // Return a local support point in a given direction without the object margin.
@ -162,23 +141,21 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
/// the capsule and return the point with the maximum dot product with the direction vector. Note /// 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. /// that the object margin is implicitly the radius and height of the capsule.
inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
// Support point top sphere // Support point top sphere
decimal dotProductTop = mHalfHeight * direction.y; float dotProductTop = mHalfHeight * direction.y;
// Support point bottom sphere // Support point bottom sphere
decimal dotProductBottom = -mHalfHeight * direction.y; float dotProductBottom = -mHalfHeight * direction.y;
// Return the point with the maximum dot product // Return the point with the maximum dot product
if (dotProductTop > dotProductBottom) { if (dotProductTop > dotProductBottom) {
return Vector3(0, mHalfHeight, 0); return Vector3(0, mHalfHeight, 0);
} }
else { else {
return Vector3(0, -mHalfHeight, 0); return Vector3(0, -mHalfHeight, 0);
} }
} }
} }
#endif

71
ephysics/collision/shapes/CollisionShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/CollisionShape.h> #include <ephysics/collision/shapes/CollisionShape.h>
@ -33,7 +14,7 @@ using namespace reactphysics3d;
// Constructor // Constructor
CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) { CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) {
} }
// Destructor // Destructor
@ -44,32 +25,32 @@ CollisionShape::~CollisionShape() {
// Compute the world-space AABB of the collision shape given a transform // Compute the world-space AABB of the collision shape given a transform
/** /**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates * computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape * @param transform Transform used to compute the AABB of the collision shape
*/ */
void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
PROFILE("CollisionShape::computeAABB()"); PROFILE("CollisionShape::computeAABB()");
// Get the local bounds in x,y and z direction // Get the local bounds in x,y and z direction
Vector3 minBounds; Vector3 minBounds;
Vector3 maxBounds; Vector3 maxBounds;
getLocalBounds(minBounds, maxBounds); getLocalBounds(minBounds, maxBounds);
// Rotate the local bounds according to the orientation of the body // Rotate the local bounds according to the orientation of the body
Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix(); Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(1).dot(minBounds),
worldAxis.getColumn(2).dot(minBounds)); worldAxis.getColumn(2).dot(minBounds));
Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
worldAxis.getColumn(1).dot(maxBounds), worldAxis.getColumn(1).dot(maxBounds),
worldAxis.getColumn(2).dot(maxBounds)); worldAxis.getColumn(2).dot(maxBounds));
// Compute the minimum and maximum coordinates of the rotated extents // Compute the minimum and maximum coordinates of the rotated extents
Vector3 minCoordinates = transform.getPosition() + worldMinBounds; Vector3 minCoordinates = transform.getPosition() + worldMinBounds;
Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds; Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds;
// Update the AABB with the new minimum and maximum coordinates // Update the AABB with the new minimum and maximum coordinates
aabb.setMin(minCoordinates); aabb.setMin(minCoordinates);
aabb.setMax(maxCoordinates); aabb.setMax(maxCoordinates);
} }

160
ephysics/collision/shapes/CollisionShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SHAPE_H
#define REACTPHYSICS3D_COLLISION_SHAPE_H
// Libraries // Libraries
#include <cassert> #include <cassert>
@ -38,11 +17,11 @@
/// ReactPhysics3D namespace /// ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
/// Type of the collision shape /// Type of the collision shape
enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER, enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9; const int32_t NB_COLLISION_SHAPE_TYPES = 9;
// Declarations // Declarations
class ProxyShape; class ProxyShape;
@ -54,76 +33,76 @@ class CollisionBody;
* body that is used during the narrow-phase collision detection. * body that is used during the narrow-phase collision detection.
*/ */
class CollisionShape { class CollisionShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Type of the collision shape /// Type of the collision shape
CollisionShapeType mType; CollisionShapeType mType;
/// Scaling vector of the collision shape /// Scaling vector of the collision shape
Vector3 mScaling; Vector3 mScaling;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
CollisionShape(const CollisionShape& shape); CollisionShape(const CollisionShape& shape);
/// Private assignment operator /// Private assignment operator
CollisionShape& operator=(const CollisionShape& shape); CollisionShape& operator=(const CollisionShape& shape);
/// Return true if a point is inside the collision shape /// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information /// 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) const=0;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0; virtual size_t getSizeInBytes() const = 0;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
CollisionShape(CollisionShapeType type); CollisionShape(CollisionShapeType type);
/// Destructor /// Destructor
virtual ~CollisionShape(); virtual ~CollisionShape();
/// Return the type of the collision shapes /// Return the type of the collision shapes
CollisionShapeType getType() const; CollisionShapeType getType() const;
/// Return true if the collision shape is convex, false if it is concave /// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const=0; virtual bool isConvex() const=0;
/// Return the local bounds of the shape in x, y and z directions /// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
/// Return the scaling vector of the collision shape /// Return the scaling vector of the collision shape
Vector3 getScaling() const; Vector3 getScaling() const;
/// Set the local scaling vector of the collision shape /// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shapes /// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const=0;
/// Compute the world-space AABB of the collision shape given a transform /// Compute the world-space AABB of the collision shape given a transform
virtual void computeAABB(AABB& aabb, const Transform& transform) const; virtual void computeAABB(AABB& aabb, const Transform& transform) const;
/// Return true if the collision shape type is a convex shape /// Return true if the collision shape type is a convex shape
static bool isConvex(CollisionShapeType shapeType); static bool isConvex(CollisionShapeType shapeType);
/// Return the maximum number of contact manifolds in an overlapping pair given two shape types /// Return the maximum number of contact manifolds in an overlapping pair given two shape types
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1, static int32_t computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2); CollisionShapeType shapeType2);
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class ProxyShape; friend class ProxyShape;
friend class CollisionWorld; friend class CollisionWorld;
}; };
// Return the type of the collision shape // Return the type of the collision shape
@ -131,37 +110,36 @@ class CollisionShape {
* @return The type of the collision shape (box, sphere, cylinder, ...) * @return The type of the collision shape (box, sphere, cylinder, ...)
*/ */
inline CollisionShapeType CollisionShape::getType() const { inline CollisionShapeType CollisionShape::getType() const {
return mType; return mType;
} }
// Return true if the collision shape type is a convex shape // Return true if the collision shape type is a convex shape
inline bool CollisionShape::isConvex(CollisionShapeType shapeType) { inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
return shapeType != CONCAVE_MESH && shapeType != HEIGHTFIELD; return shapeType != CONCAVE_MESH && shapeType != HEIGHTFIELD;
} }
// Return the scaling vector of the collision shape // Return the scaling vector of the collision shape
inline Vector3 CollisionShape::getScaling() const { inline Vector3 CollisionShape::getScaling() const {
return mScaling; return mScaling;
} }
// Set the scaling vector of the collision shape // Set the scaling vector of the collision shape
inline void CollisionShape::setLocalScaling(const Vector3& scaling) { inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling; mScaling = scaling;
} }
// Return the maximum number of contact manifolds allowed in an overlapping // Return the maximum number of contact manifolds allowed in an overlapping
// pair wit the given two collision shape types // pair wit the given two collision shape types
inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, inline int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2) { CollisionShapeType shapeType2) {
// If both shapes are convex // If both shapes are convex
if (isConvex(shapeType1) && isConvex(shapeType2)) { if (isConvex(shapeType1) && isConvex(shapeType2)) {
return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
} // If there is at least one concave shape } // If there is at least one concave shape
else { else {
return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
} }
} }
} }
#endif

101
ephysics/collision/shapes/ConcaveMeshShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/ConcaveMeshShape.h> #include <ephysics/collision/shapes/ConcaveMeshShape.h>
@ -34,7 +15,7 @@ ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh):
ConcaveShape(CONCAVE_MESH) { ConcaveShape(CONCAVE_MESH) {
m_triangleMesh = triangleMesh; m_triangleMesh = triangleMesh;
m_raycastTestType = FRONT; m_raycastTestType = FRONT;
// Insert all the triangles into the dynamic AABB tree // Insert all the triangles int32_to the dynamic AABB tree
initBVHTree(); initBVHTree();
} }
@ -43,29 +24,29 @@ ConcaveMeshShape::~ConcaveMeshShape() {
} }
// Insert all the triangles into the dynamic AABB tree // Insert all the triangles int32_to the dynamic AABB tree
void ConcaveMeshShape::initBVHTree() { void ConcaveMeshShape::initBVHTree() {
// TODO : Try to randomly add the triangles into the tree to obtain a better tree // TODO : Try to randomly add the triangles int32_to the tree to obtain a better tree
// For each sub-part of the mesh // For each sub-part of the mesh
for (uint subPart=0; subPart<m_triangleMesh->getNbSubparts(); subPart++) { for (uint32_t subPart=0; subPart<m_triangleMesh->getNbSubparts(); subPart++) {
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart); TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride(); int32_t vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride(); int32_t indexStride = triangleVertexArray->getIndicesStride();
// For each triangle of the concave mesh // For each triangle of the concave mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) { for (uint32_t triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
Vector3 trianglePoints[3]; Vector3 trianglePoints[3];
// For each vertex of the triangle // For each vertex of the triangle
for (int k=0; k < 3; k++) { for (int32_t k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle // Get the index of the current vertex in the triangle
int vertexIndex = 0; int32_t vertexIndex = 0;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k]; vertexIndex = ((uint32_t*)vertexIndexPointer)[k];
} else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { } else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
} else { } else {
@ -74,14 +55,14 @@ void ConcaveMeshShape::initBVHTree() {
// Get the vertices components of the triangle // Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; trianglePoints[k][0] = float(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; trianglePoints[k][1] = float(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; trianglePoints[k][2] = float(vertices[2]) * mScaling.z;
} else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { } else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; trianglePoints[k][0] = float(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; trianglePoints[k][1] = float(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; trianglePoints[k][2] = float(vertices[2]) * mScaling.z;
} else { } else {
assert(false); assert(false);
} }
@ -89,7 +70,7 @@ void ConcaveMeshShape::initBVHTree() {
// Create the AABB for the triangle // Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints); AABB aabb = AABB::createAABBForTriangle(trianglePoints);
aabb.inflate(m_triangleMargin, m_triangleMargin, m_triangleMargin); aabb.inflate(m_triangleMargin, m_triangleMargin, m_triangleMargin);
// Add the AABB with the index of the triangle into the dynamic AABB tree // Add the AABB with the index of the triangle int32_to the dynamic AABB tree
m_dynamicAABBTree.addObject(aabb, subPart, triangleIndex); m_dynamicAABBTree.addObject(aabb, subPart, triangleIndex);
} }
} }
@ -97,7 +78,7 @@ void ConcaveMeshShape::initBVHTree() {
// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle // Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
// given the start vertex index pointer of the triangle // given the start vertex index pointer of the triangle
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, Vector3* outTriangleVertices) const { void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, Vector3* outTriangleVertices) const {
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart); TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart);
if (triangleVertexArray == nullptr) { if (triangleVertexArray == nullptr) {
@ -107,15 +88,15 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride(); int32_t vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride(); int32_t indexStride = triangleVertexArray->getIndicesStride();
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
// For each vertex of the triangle // For each vertex of the triangle
for (int k=0; k < 3; k++) { for (int32_t k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle // Get the index of the current vertex in the triangle
int vertexIndex = 0; int32_t vertexIndex = 0;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k]; vertexIndex = ((uint32_t*)vertexIndexPointer)[k];
} else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { } else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
} else { } else {
@ -125,14 +106,14 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32
// Get the vertices components of the triangle // Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z;
} else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { } else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z;
} else { } else {
assert(false); assert(false);
} }
@ -163,24 +144,24 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
} }
// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree // Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { float ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) {
// Add the id of the hit AABB node into // Add the id of the hit AABB node int32_to
m_hitAABBNodes.push_back(nodeId); m_hitAABBNodes.push_back(nodeId);
return ray.maxFraction; return ray.maxFraction;
} }
// Raycast all collision shapes that have been collected // Raycast all collision shapes that have been collected
void ConcaveMeshRaycastCallback::raycastTriangles() { void ConcaveMeshRaycastCallback::raycastTriangles() {
std::vector<int>::const_iterator it; std::vector<int32_t>::const_iterator it;
decimal smallestHitFraction = m_ray.maxFraction; float smallestHitFraction = m_ray.maxFraction;
for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) { for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) {
// Get the node data (triangle index and mesh subpart index) // Get the node data (triangle index and mesh subpart index)
int32* data = m_dynamicAABBTree.getNodeDataInt(*it); int32_t* data = m_dynamicAABBTree.getNodeDataInt(*it);
// Get the triangle vertices for this node from the concave mesh shape // Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3]; Vector3 trianglePoints[3];
m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Create a triangle collision shape // Create a triangle collision shape
decimal margin = m_concaveMeshShape.getTriangleMargin(); float margin = m_concaveMeshShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
triangleShape.setRaycastTestType(m_concaveMeshShape.getRaycastTestType()); triangleShape.setRaycastTestType(m_concaveMeshShape.getRaycastTestType());
// Ray casting test against the collision shape // Ray casting test against the collision shape
@ -188,7 +169,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape);
// If the ray hit the collision shape // If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
assert(raycastInfo.hitFraction >= decimal(0.0)); assert(raycastInfo.hitFraction >= float(0.0));
m_raycastInfo.body = raycastInfo.body; m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape; m_raycastInfo.proxyShape = raycastInfo.proxyShape;
m_raycastInfo.hitFraction = raycastInfo.hitFraction; m_raycastInfo.hitFraction = raycastInfo.hitFraction;

239
ephysics/collision/shapes/ConcaveMeshShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_MESH_SHAPE_H
#define REACTPHYSICS3D_CONCAVE_MESH_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConcaveShape.h> #include <ephysics/collision/shapes/ConcaveShape.h>
@ -40,64 +19,64 @@ class ConcaveMeshShape;
// class ConvexTriangleAABBOverlapCallback // class ConvexTriangleAABBOverlapCallback
class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback { class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private: private:
TriangleCallback& m_triangleTestCallback; TriangleCallback& m_triangleTestCallback;
// Reference to the concave mesh shape // Reference to the concave mesh shape
const ConcaveMeshShape& m_concaveMeshShape; const ConcaveMeshShape& m_concaveMeshShape;
// Reference to the Dynamic AABB tree // Reference to the Dynamic AABB tree
const DynamicAABBTree& m_dynamicAABBTree; const DynamicAABBTree& m_dynamicAABBTree;
public: public:
// Constructor // Constructor
ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape, ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape,
const DynamicAABBTree& dynamicAABBTree) const DynamicAABBTree& dynamicAABBTree)
: m_triangleTestCallback(triangleCallback), m_concaveMeshShape(concaveShape), m_dynamicAABBTree(dynamicAABBTree) { : m_triangleTestCallback(triangleCallback), m_concaveMeshShape(concaveShape), m_dynamicAABBTree(dynamicAABBTree) {
} }
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId); virtual void notifyOverlappingNode(int32_t nodeId);
}; };
/// Class ConcaveMeshRaycastCallback /// Class ConcaveMeshRaycastCallback
class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
private : private :
std::vector<int32> m_hitAABBNodes; std::vector<int32_t> m_hitAABBNodes;
const DynamicAABBTree& m_dynamicAABBTree; const DynamicAABBTree& m_dynamicAABBTree;
const ConcaveMeshShape& m_concaveMeshShape; const ConcaveMeshShape& m_concaveMeshShape;
ProxyShape* m_proxyShape; ProxyShape* m_proxyShape;
RaycastInfo& m_raycastInfo; RaycastInfo& m_raycastInfo;
const Ray& m_ray; const Ray& m_ray;
bool mIsHit; bool mIsHit;
public: public:
// Constructor // Constructor
ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray) ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray)
: m_dynamicAABBTree(dynamicAABBTree), m_concaveMeshShape(concaveMeshShape), m_proxyShape(proxyShape), : m_dynamicAABBTree(dynamicAABBTree), m_concaveMeshShape(concaveMeshShape), m_proxyShape(proxyShape),
m_raycastInfo(raycastInfo), m_ray(ray), mIsHit(false) { m_raycastInfo(raycastInfo), m_ray(ray), mIsHit(false) {
} }
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree /// 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 float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray);
/// Raycast all collision shapes that have been collected /// Raycast all collision shapes that have been collected
void raycastTriangles(); void raycastTriangles();
/// Return true if a raycast hit has been found /// Return true if a raycast hit has been found
bool getIsHit() const { bool getIsHit() const {
return mIsHit; return mIsHit;
} }
}; };
// Class ConcaveMeshShape // Class ConcaveMeshShape
@ -108,67 +87,67 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
*/ */
class ConcaveMeshShape : public ConcaveShape { class ConcaveMeshShape : public ConcaveShape {
protected: protected:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Triangle mesh /// Triangle mesh
TriangleMesh* m_triangleMesh; TriangleMesh* m_triangleMesh;
/// Dynamic AABB tree to accelerate collision with the triangles /// Dynamic AABB tree to accelerate collision with the triangles
DynamicAABBTree m_dynamicAABBTree; DynamicAABBTree m_dynamicAABBTree;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ConcaveMeshShape(const ConcaveMeshShape& shape); ConcaveMeshShape(const ConcaveMeshShape& shape);
/// Private assignment operator /// Private assignment operator
ConcaveMeshShape& operator=(const ConcaveMeshShape& shape); ConcaveMeshShape& operator=(const ConcaveMeshShape& shape);
/// Raycast method with feedback information /// 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) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Insert all the triangles into the dynamic AABB tree /// Insert all the triangles int32_to the dynamic AABB tree
void initBVHTree(); void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle. /// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex,
Vector3* outTriangleVertices) const; Vector3* outTriangleVertices) const;
public: public:
/// Constructor /// Constructor
ConcaveMeshShape(TriangleMesh* triangleMesh); ConcaveMeshShape(TriangleMesh* triangleMesh);
/// Destructor /// Destructor
~ConcaveMeshShape(); ~ConcaveMeshShape();
/// Return the local bounds of the shape in x, y and z directions. /// 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;
/// Set the local scaling vector of the collision shape /// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB /// 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;
// ---------- Friendship ----------- // // ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback; friend class ConvexTriangleAABBOverlapCallback;
friend class ConcaveMeshRaycastCallback; friend class ConcaveMeshRaycastCallback;
}; };
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t ConcaveMeshShape::getSizeInBytes() const { inline size_t ConcaveMeshShape::getSizeInBytes() const {
return sizeof(ConcaveMeshShape); return sizeof(ConcaveMeshShape);
} }
// Return the local bounds of the shape in x, y and z directions. // Return the local bounds of the shape in x, y and z directions.
@ -179,57 +158,55 @@ inline size_t ConcaveMeshShape::getSizeInBytes() const {
*/ */
inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Get the AABB of the whole tree // Get the AABB of the whole tree
AABB treeAABB = m_dynamicAABBTree.getRootAABB(); AABB treeAABB = m_dynamicAABBTree.getRootAABB();
min = treeAABB.getMin(); min = treeAABB.getMin();
max = treeAABB.getMax(); max = treeAABB.getMax();
} }
// Set the local scaling vector of the collision shape // Set the local scaling vector of the collision shape
inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) { inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) {
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
// Reset the Dynamic AABB Tree // Reset the Dynamic AABB Tree
m_dynamicAABBTree.reset(); m_dynamicAABBTree.reset();
// Rebuild Dynamic AABB Tree here // Rebuild Dynamic AABB Tree here
initBVHTree(); initBVHTree();
} }
// Return the local inertia tensor of the shape // Return the local inertia tensor of the shape
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
// Default inertia tensor // Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh. // Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore, // However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used. // the inertia tensor is not used.
tensor.setAllValues(mass, 0, 0, tensor.setAllValues(mass, 0, 0,
0, mass, 0, 0, mass, 0,
0, 0, mass); 0, 0, mass);
} }
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) { inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) {
// Get the node data (triangle index and mesh subpart index) // Get the node data (triangle index and mesh subpart index)
int32* data = m_dynamicAABBTree.getNodeDataInt(nodeId); int32_t* data = m_dynamicAABBTree.getNodeDataInt(nodeId);
// Get the triangle vertices for this node from the concave mesh shape // Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3]; Vector3 trianglePoints[3];
m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle // Call the callback to test narrow-phase collision with this triangle
m_triangleTestCallback.testTriangle(trianglePoints); m_triangleTestCallback.testTriangle(trianglePoints);
} }
} }
#endif

33
ephysics/collision/shapes/ConcaveShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/ConcaveShape.h> #include <ephysics/collision/shapes/ConcaveShape.h>
@ -32,8 +13,8 @@ using namespace reactphysics3d;
// Constructor // Constructor
ConcaveShape::ConcaveShape(CollisionShapeType type) ConcaveShape::ConcaveShape(CollisionShapeType type)
: CollisionShape(type), m_isSmoothMeshCollisionEnabled(false), : CollisionShape(type), m_isSmoothMeshCollisionEnabled(false),
m_triangleMargin(0), m_raycastTestType(FRONT) { m_triangleMargin(0), m_raycastTestType(FRONT) {
} }

130
ephysics/collision/shapes/ConcaveShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SHAPE_H
#define REACTPHYSICS3D_CONCAVE_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/CollisionShape.h> #include <ephysics/collision/shapes/CollisionShape.h>
@ -40,11 +19,11 @@ namespace reactphysics3d {
*/ */
class TriangleCallback { class TriangleCallback {
public: public:
virtual ~TriangleCallback() = default; virtual ~TriangleCallback() = default;
/// Report a triangle /// Report a triangle
virtual void testTriangle(const Vector3* trianglePoints)=0; virtual void testTriangle(const Vector3* trianglePoints)=0;
}; };
@ -56,93 +35,93 @@ class TriangleCallback {
*/ */
class ConcaveShape : public CollisionShape { class ConcaveShape : public CollisionShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// True if the smooth mesh collision algorithm is enabled /// True if the smooth mesh collision algorithm is enabled
bool m_isSmoothMeshCollisionEnabled; bool m_isSmoothMeshCollisionEnabled;
// Margin use for collision detection for each triangle // Margin use for collision detection for each triangle
decimal m_triangleMargin; float m_triangleMargin;
/// Raycast test type for the triangle (front, back, front-back) /// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide m_raycastTestType; TriangleRaycastSide m_raycastTestType;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ConcaveShape(const ConcaveShape& shape); ConcaveShape(const ConcaveShape& shape);
/// Private assignment operator /// Private assignment operator
ConcaveShape& operator=(const ConcaveShape& shape); ConcaveShape& operator=(const ConcaveShape& shape);
/// Return true if a point is inside the collision shape /// 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;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConcaveShape(CollisionShapeType type); ConcaveShape(CollisionShapeType type);
/// Destructor /// Destructor
virtual ~ConcaveShape(); virtual ~ConcaveShape();
/// Return the triangle margin /// Return the triangle margin
decimal getTriangleMargin() const; float getTriangleMargin() const;
/// Return the raycast test type (front, back, front-back) /// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const; TriangleRaycastSide getRaycastTestType() const;
// Set the raycast test type (front, back, front-back) // Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType); void setRaycastTestType(TriangleRaycastSide testType);
/// Return true if the collision shape is convex, false if it is concave /// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const; virtual bool isConvex() const;
/// Use a callback method on all triangles of the concave shape inside a given AABB /// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0; virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
/// Return true if the smooth mesh collision is enabled /// Return true if the smooth mesh collision is enabled
bool getIsSmoothMeshCollisionEnabled() const; bool getIsSmoothMeshCollisionEnabled() const;
/// Enable/disable the smooth mesh collision algorithm /// Enable/disable the smooth mesh collision algorithm
void setIsSmoothMeshCollisionEnabled(bool isEnabled); void setIsSmoothMeshCollisionEnabled(bool isEnabled);
}; };
// Return the triangle margin // Return the triangle margin
inline decimal ConcaveShape::getTriangleMargin() const { inline float ConcaveShape::getTriangleMargin() const {
return m_triangleMargin; return m_triangleMargin;
} }
/// Return true if the collision shape is convex, false if it is concave /// Return true if the collision shape is convex, false if it is concave
inline bool ConcaveShape::isConvex() const { inline bool ConcaveShape::isConvex() const {
return false; return false;
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return false; return false;
} }
// Return true if the smooth mesh collision is enabled // Return true if the smooth mesh collision is enabled
inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const { inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
return m_isSmoothMeshCollisionEnabled; return m_isSmoothMeshCollisionEnabled;
} }
// Enable/disable the smooth mesh collision algorithm // Enable/disable the smooth mesh collision algorithm
/// Smooth mesh collision is used to avoid collisions against some internal edges /// Smooth mesh collision is used to avoid collisions against some int32_ternal edges
/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother /// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother
/// but collisions computation is a bit more expensive. /// but collisions computation is a bit more expensive.
inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) { inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
m_isSmoothMeshCollisionEnabled = isEnabled; m_isSmoothMeshCollisionEnabled = isEnabled;
} }
// Return the raycast test type (front, back, front-back) // Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const { inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const {
return m_raycastTestType; return m_raycastTestType;
} }
// Set the raycast test type (front, back, front-back) // Set the raycast test type (front, back, front-back)
@ -150,10 +129,9 @@ inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const {
* @param testType Raycast test type for the triangle (front, back, front-back) * @param testType Raycast test type for the triangle (front, back, front-back)
*/ */
inline void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) { inline void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) {
m_raycastTestType = testType; m_raycastTestType = testType;
} }
} }
#endif

295
ephysics/collision/shapes/ConeShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <complex> #include <complex>
@ -37,13 +18,13 @@ using namespace reactphysics3d;
* @param height Height of the cone (in meters) * @param height Height of the cone (in meters)
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
ConeShape::ConeShape(decimal radius, decimal height, decimal margin) ConeShape::ConeShape(float radius, float height, float margin)
: ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) { : ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * float(0.5)) {
assert(mRadius > decimal(0.0)); assert(mRadius > float(0.0));
assert(mHalfHeight > decimal(0.0)); assert(mHalfHeight > float(0.0));
// Compute the sine of the semi-angle at the apex point // Compute the sine of the semi-angle at the apex point
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
} }
// Destructor // Destructor
@ -53,27 +34,27 @@ ConeShape::~ConeShape() {
// Return a local support point in a given direction without the object margin // Return a local support point in a given direction without the object margin
Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
const Vector3& v = direction; const Vector3& v = direction;
decimal sinThetaTimesLengthV = mSinTheta * v.length(); float sinThetaTimesLengthV = mSinTheta * v.length();
Vector3 supportPoint; Vector3 supportPoint;
if (v.y > sinThetaTimesLengthV) { if (v.y > sinThetaTimesLengthV) {
supportPoint = Vector3(0.0, mHalfHeight, 0.0); supportPoint = Vector3(0.0, mHalfHeight, 0.0);
} }
else { else {
decimal projectedLength = sqrt(v.x * v.x + v.z * v.z); float projectedLength = sqrt(v.x * v.x + v.z * v.z);
if (projectedLength > MACHINE_EPSILON) { if (projectedLength > MACHINE_EPSILON) {
decimal d = mRadius / projectedLength; float d = mRadius / projectedLength;
supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d); supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d);
} }
else { else {
supportPoint = Vector3(0.0, -mHalfHeight, 0.0); supportPoint = Vector3(0.0, -mHalfHeight, 0.0);
} }
} }
return supportPoint; return supportPoint;
} }
// Raycast method with feedback information // Raycast method with feedback information
@ -82,133 +63,133 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf // http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 r = ray.point2 - ray.point1; const Vector3 r = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.00001); const float epsilon = float(0.00001);
Vector3 V(0, mHalfHeight, 0); Vector3 V(0, mHalfHeight, 0);
Vector3 centerBase(0, -mHalfHeight, 0); Vector3 centerBase(0, -mHalfHeight, 0);
Vector3 axis(0, decimal(-1.0), 0); Vector3 axis(0, float(-1.0), 0);
decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight; float heightSquare = float(4.0) * mHalfHeight * mHalfHeight;
decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); float cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
decimal factor = decimal(1.0) - cosThetaSquare; float factor = float(1.0) - cosThetaSquare;
Vector3 delta = ray.point1 - V; Vector3 delta = ray.point1 - V;
decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - float c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y -
cosThetaSquare * delta.z * delta.z; cosThetaSquare * delta.z * delta.z;
decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z; float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)}; float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
Vector3 localHitPoint[3]; Vector3 localHitPoint[3];
Vector3 localNormal[3]; Vector3 localNormal[3];
// If c2 is different from zero // If c2 is different from zero
if (std::abs(c2) > MACHINE_EPSILON) { if (std::abs(c2) > MACHINE_EPSILON) {
decimal gamma = c1 * c1 - c0 * c2; float gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation // If there is no real roots in the quadratic equation
if (gamma < decimal(0.0)) { if (gamma < float(0.0)) {
return false; return false;
} }
else if (gamma > decimal(0.0)) { // The equation has two real roots else if (gamma > float(0.0)) { // The equation has two real roots
// Compute two intersections // Compute two int32_tersections
decimal sqrRoot = std::sqrt(gamma); float sqrRoot = std::sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2; tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2; tHit[1] = (-c1 + sqrRoot) / c2;
} }
else { // If the equation has a single real root else { // If the equation has a single real root
// Compute the intersection // Compute the int32_tersection
tHit[0] = -c1 / c2; tHit[0] = -c1 / c2;
} }
} }
else { // If c2 == 0 else { // If c2 == 0
// If c2 = 0 and c1 != 0 // If c2 = 0 and c1 != 0
if (std::abs(c1) > MACHINE_EPSILON) { if (std::abs(c1) > MACHINE_EPSILON) {
tHit[0] = -c0 / (decimal(2.0) * c1); tHit[0] = -c0 / (float(2.0) * c1);
} }
else { // If c2 = c1 = 0 else { // If c2 = c1 = 0
// If c0 is different from zero, no solution and if c0 = 0, we have a // If c0 is different from zero, no solution and if c0 = 0, we have a
// degenerate case, the whole ray is contained in the cone side // degenerate case, the whole ray is contained in the cone side
// but we return no hit in this case // but we return no hit in this case
return false; return false;
} }
} }
// If the origin of the ray is inside the cone, we return no hit // If the origin of the ray is inside the cone, we return no hit
if (testPointInside(ray.point1, NULL)) return false; if (testPointInside(ray.point1, NULL)) return false;
localHitPoint[0] = ray.point1 + tHit[0] * r; localHitPoint[0] = ray.point1 + tHit[0] * r;
localHitPoint[1] = ray.point1 + tHit[1] * r; localHitPoint[1] = ray.point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are interested in) // Only keep hit points in one side of the double cone (the cone we are int32_terested in)
if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) { if (axis.dot(localHitPoint[0] - V) < float(0.0)) {
tHit[0] = decimal(-1.0); tHit[0] = float(-1.0);
} }
if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) { if (axis.dot(localHitPoint[1] - V) < float(0.0)) {
tHit[1] = decimal(-1.0); tHit[1] = float(-1.0);
} }
// Only keep hit points that are within the correct height of the cone // Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y < decimal(-mHalfHeight)) { if (localHitPoint[0].y < float(-mHalfHeight)) {
tHit[0] = decimal(-1.0); tHit[0] = float(-1.0);
} }
if (localHitPoint[1].y < decimal(-mHalfHeight)) { if (localHitPoint[1].y < float(-mHalfHeight)) {
tHit[1] = decimal(-1.0); tHit[1] = float(-1.0);
} }
// If the ray is in direction of the base plane of the cone // If the ray is in direction of the base plane of the cone
if (r.y > epsilon) { if (r.y > epsilon) {
// Compute the intersection with the base plane of the cone // Compute the int32_tersection with the base plane of the cone
tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y); tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y);
// Only keep this intersection if it is inside the cone radius // Only keep this int32_tersection if it is inside the cone radius
localHitPoint[2] = ray.point1 + tHit[2] * r; localHitPoint[2] = ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) { if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
tHit[2] = decimal(-1.0); tHit[2] = float(-1.0);
} }
// Compute the normal direction // Compute the normal direction
localNormal[2] = axis; localNormal[2] = axis;
} }
// Find the smallest positive t value // Find the smallest positive t value
int hitIndex = -1; int32_t hitIndex = -1;
decimal t = DECIMAL_LARGEST; float t = DECIMAL_LARGEST;
for (int i=0; i<3; i++) { for (int32_t i=0; i<3; i++) {
if (tHit[i] < decimal(0.0)) continue; if (tHit[i] < float(0.0)) continue;
if (tHit[i] < t) { if (tHit[i] < t) {
hitIndex = i; hitIndex = i;
t = tHit[hitIndex]; t = tHit[hitIndex];
} }
} }
if (hitIndex < 0) return false; if (hitIndex < 0) return false;
if (t > ray.maxFraction) return false; if (t > ray.maxFraction) return false;
// Compute the normal direction for hit against side of the cone // Compute the normal direction for hit against side of the cone
if (hitIndex != 2) { if (hitIndex != 2) {
decimal h = decimal(2.0) * mHalfHeight; float h = float(2.0) * mHalfHeight;
decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
localHitPoint[hitIndex].z * localHitPoint[hitIndex].z); localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
decimal rOverH = mRadius / h; float rOverH = mRadius / h;
decimal value2 = decimal(1.0) + rOverH * rOverH; float value2 = float(1.0) + rOverH * rOverH;
decimal factor = decimal(1.0) / std::sqrt(value1 * value2); float factor = float(1.0) / std::sqrt(value1 * value2);
decimal x = localHitPoint[hitIndex].x * factor; float x = localHitPoint[hitIndex].x * factor;
decimal z = localHitPoint[hitIndex].z * factor; float z = localHitPoint[hitIndex].z * factor;
localNormal[hitIndex].x = x; localNormal[hitIndex].x = x;
localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH; localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH;
localNormal[hitIndex].z = z; localNormal[hitIndex].z = z;
} }
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint[hitIndex]; raycastInfo.worldPoint = localHitPoint[hitIndex];
raycastInfo.worldNormal = localNormal[hitIndex]; raycastInfo.worldNormal = localNormal[hitIndex];
return true; return true;
} }

167
ephysics/collision/shapes/ConeShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONE_SHAPE_H
#define REACTPHYSICS3D_CONE_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -51,94 +30,94 @@ namespace reactphysics3d {
*/ */
class ConeShape : public ConvexShape { class ConeShape : public ConvexShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Radius of the base /// Radius of the base
decimal mRadius; float mRadius;
/// Half height of the cone /// Half height of the cone
decimal mHalfHeight; float mHalfHeight;
/// sine of the semi angle at the apex point /// sine of the semi angle at the apex point
decimal mSinTheta; float mSinTheta;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ConeShape(const ConeShape& shape); ConeShape(const ConeShape& shape);
/// Private assignment operator /// Private assignment operator
ConeShape& operator=(const ConeShape& shape); ConeShape& operator=(const ConeShape& shape);
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// 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;
/// Raycast method with feedback information /// 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) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN); ConeShape(float mRadius, float height, float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~ConeShape(); virtual ~ConeShape();
/// Return the radius /// Return the radius
decimal getRadius() const; float getRadius() const;
/// Return the height /// Return the height
decimal getHeight() const; float getHeight() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions /// 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;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
}; };
// Return the radius // Return the radius
/** /**
* @return Radius of the cone (in meters) * @return Radius of the cone (in meters)
*/ */
inline decimal ConeShape::getRadius() const { inline float ConeShape::getRadius() const {
return mRadius; return mRadius;
} }
// Return the height // Return the height
/** /**
* @return Height of the cone (in meters) * @return Height of the cone (in meters)
*/ */
inline decimal ConeShape::getHeight() const { inline float ConeShape::getHeight() const {
return decimal(2.0) * mHalfHeight; return float(2.0) * mHalfHeight;
} }
// Set the scaling vector of the collision shape // Set the scaling vector of the collision shape
inline void ConeShape::setLocalScaling(const Vector3& scaling) { inline void ConeShape::setLocalScaling(const Vector3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mRadius = (mRadius / mScaling.x) * scaling.x; mRadius = (mRadius / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t ConeShape::getSizeInBytes() const { inline size_t ConeShape::getSizeInBytes() const {
return sizeof(ConeShape); return sizeof(ConeShape);
} }
// Return the local bounds of the shape in x, y and z directions // Return the local bounds of the shape in x, y and z directions
@ -148,39 +127,37 @@ inline size_t ConeShape::getSizeInBytes() const {
*/ */
inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds // Maximum bounds
max.x = mRadius + mMargin; max.x = mRadius + mMargin;
max.y = mHalfHeight + mMargin; max.y = mHalfHeight + mMargin;
max.z = max.x; max.z = max.x;
// Minimum bounds // Minimum bounds
min.x = -max.x; min.x = -max.x;
min.y = -max.y; min.y = -max.y;
min.z = min.x; min.z = min.x;
} }
// Return the local inertia tensor of the collision shape // Return the local inertia tensor of the collision shape
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal rSquare = mRadius * mRadius; float rSquare = mRadius * mRadius;
decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight); float diagXZ = float(0.15) * mass * (rSquare + mHalfHeight);
tensor.setAllValues(diagXZ, 0.0, 0.0, tensor.setAllValues(diagXZ, 0.0, 0.0,
0.0, decimal(0.3) * mass * rSquare, 0.0, float(0.3) * mass * rSquare,
0.0, 0.0, 0.0, diagXZ); 0.0, 0.0, 0.0, diagXZ);
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) / const float radiusHeight = mRadius * (-localPoint.y + mHalfHeight) /
(mHalfHeight * decimal(2.0)); (mHalfHeight * float(2.0));
return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) && return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) &&
(localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight); (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight);
} }
} }
#endif

323
ephysics/collision/shapes/ConvexMeshShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <complex> #include <complex>
@ -31,112 +12,112 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor to initialize with an array of 3D vertices. // Constructor to initialize with an array of 3D vertices.
/// This method creates an internal copy of the input vertices. /// This method creates an int32_ternal copy of the input vertices.
/** /**
* @param arrayVertices Array with the vertices of the convex mesh * @param arrayVertices Array with the vertices of the convex mesh
* @param nbVertices Number of vertices in the convex mesh * @param nbVertices Number of vertices in the convex mesh
* @param stride Stride between the beginning of two elements in the vertices array * @param stride Stride between the beginning of two elements in the vertices array
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin) ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, float margin)
: ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0), : ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) {
assert(nbVertices > 0); assert(nbVertices > 0);
assert(stride > 0); assert(stride > 0);
const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
// Copy all the vertices into the internal array // Copy all the vertices int32_to the int32_ternal array
for (uint i=0; i<m_numberVertices; i++) { for (uint32_t i=0; i<m_numberVertices; i++) {
const decimal* newPoint = (const decimal*) vertexPointer; const float* newPoint = (const float*) vertexPointer;
m_vertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2])); m_vertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride; vertexPointer += stride;
} }
// Recalculate the bounds of the mesh // Recalculate the bounds of the mesh
recalculateBounds(); recalculateBounds();
} }
// Constructor to initialize with a triangle mesh // Constructor to initialize with a triangle mesh
/// This method creates an internal copy of the input vertices. /// This method creates an int32_ternal copy of the input vertices.
/** /**
* @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh * @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
* @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory) * @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, decimal margin) ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, float margin)
: ConvexShape(CONVEX_MESH, margin), m_minBounds(0, 0, 0), : ConvexShape(CONVEX_MESH, margin), m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(isEdgesInformationUsed) { m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(isEdgesInformationUsed) {
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride(); int32_t vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride(); int32_t indexStride = triangleVertexArray->getIndicesStride();
// For each vertex of the mesh // For each vertex of the mesh
for (uint v = 0; v < triangleVertexArray->getNbVertices(); v++) { for (uint32_t v = 0; v < triangleVertexArray->getNbVertices(); v++) {
// Get the vertices components of the triangle // Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + v * vertexStride); const float* vertices = (float*)(verticesStart + v * vertexStride);
Vector3 vertex(vertices[0], vertices[1], vertices[2] ); Vector3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * mScaling; vertex = vertex * mScaling;
m_vertices.push_back(vertex); m_vertices.push_back(vertex);
} }
else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + v * vertexStride); const double* vertices = (double*)(verticesStart + v * vertexStride);
Vector3 vertex(vertices[0], vertices[1], vertices[2] ); Vector3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * mScaling; vertex = vertex * mScaling;
m_vertices.push_back(vertex); m_vertices.push_back(vertex);
} }
} }
// If we need to use the edges information of the mesh // If we need to use the edges information of the mesh
if (m_isEdgesInformationUsed) { if (m_isEdgesInformationUsed) {
// For each triangle of the mesh // For each triangle of the mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) { for (uint32_t triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
uint vertexIndex[3] = {0, 0, 0}; uint32_t vertexIndex[3] = {0, 0, 0};
// For each vertex of the triangle // For each vertex of the triangle
for (int k=0; k < 3; k++) { for (int32_t k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle // Get the index of the current vertex in the triangle
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex[k] = ((uint*)vertexIndexPointer)[k]; vertexIndex[k] = ((uint32_t*)vertexIndexPointer)[k];
} }
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k]; vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
} }
else { else {
assert(false); assert(false);
} }
} }
// Add information about the edges // Add information about the edges
addEdge(vertexIndex[0], vertexIndex[1]); addEdge(vertexIndex[0], vertexIndex[1]);
addEdge(vertexIndex[0], vertexIndex[2]); addEdge(vertexIndex[0], vertexIndex[2]);
addEdge(vertexIndex[1], vertexIndex[2]); addEdge(vertexIndex[1], vertexIndex[2]);
} }
} }
m_numberVertices = m_vertices.size(); m_numberVertices = m_vertices.size();
recalculateBounds(); recalculateBounds();
} }
// Constructor. // Constructor.
/// If you use this constructor, you will need to set the vertices manually one by one using /// If you use this constructor, you will need to set the vertices manually one by one using
/// the addVertex() method. /// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(decimal margin) ConvexMeshShape::ConvexMeshShape(float margin)
: ConvexShape(CONVEX_MESH, margin), m_numberVertices(0), m_minBounds(0, 0, 0), : ConvexShape(CONVEX_MESH, margin), m_numberVertices(0), m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) {
} }
@ -154,115 +135,115 @@ ConvexMeshShape::~ConvexMeshShape() {
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method /// will be in most of the cases very close to the previous one. Using hill-climbing, this method
/// runs in almost constant time. /// runs in almost constant time.
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction, Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
assert(m_numberVertices == m_vertices.size()); assert(m_numberVertices == m_vertices.size());
assert(cachedCollisionData != NULL); assert(cachedCollisionData != NULL);
// Allocate memory for the cached collision data if not allocated yet // Allocate memory for the cached collision data if not allocated yet
if ((*cachedCollisionData) == NULL) { if ((*cachedCollisionData) == NULL) {
*cachedCollisionData = (int*) malloc(sizeof(int)); *cachedCollisionData = (int32_t*) malloc(sizeof(int32_t));
*((int*)(*cachedCollisionData)) = 0; *((int32_t*)(*cachedCollisionData)) = 0;
} }
// If the edges information is used to speed up the collision detection // If the edges information is used to speed up the collision detection
if (m_isEdgesInformationUsed) { if (m_isEdgesInformationUsed) {
assert(m_edgesAdjacencyList.size() == m_numberVertices); assert(m_edgesAdjacencyList.size() == m_numberVertices);
uint maxVertex = *((int*)(*cachedCollisionData)); uint32_t maxVertex = *((int32_t*)(*cachedCollisionData));
decimal maxDotProduct = direction.dot(m_vertices[maxVertex]); float maxDotProduct = direction.dot(m_vertices[maxVertex]);
bool isOptimal; bool isOptimal;
// Perform hill-climbing (local search) // Perform hill-climbing (local search)
do { do {
isOptimal = true; isOptimal = true;
assert(m_edgesAdjacencyList.at(maxVertex).size() > 0); assert(m_edgesAdjacencyList.at(maxVertex).size() > 0);
// For all neighbors of the current vertex // For all neighbors of the current vertex
std::set<uint>::const_iterator it; std::set<uint32_t>::const_iterator it;
std::set<uint>::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin(); std::set<uint32_t>::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin();
std::set<uint>::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end(); std::set<uint32_t>::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end();
for (it = itBegin; it != itEnd; ++it) { for (it = itBegin; it != itEnd; ++it) {
// Compute the dot product // Compute the dot product
decimal dotProduct = direction.dot(m_vertices[*it]); float dotProduct = direction.dot(m_vertices[*it]);
// If the current vertex is a better vertex (larger dot product) // If the current vertex is a better vertex (larger dot product)
if (dotProduct > maxDotProduct) { if (dotProduct > maxDotProduct) {
maxVertex = *it; maxVertex = *it;
maxDotProduct = dotProduct; maxDotProduct = dotProduct;
isOptimal = false; isOptimal = false;
} }
} }
} while(!isOptimal); } while(!isOptimal);
// Cache the support vertex // Cache the support vertex
*((int*)(*cachedCollisionData)) = maxVertex; *((int32_t*)(*cachedCollisionData)) = maxVertex;
// Return the support vertex // Return the support vertex
return m_vertices[maxVertex] * mScaling; return m_vertices[maxVertex] * mScaling;
} }
else { // If the edges information is not used else { // If the edges information is not used
double maxDotProduct = DECIMAL_SMALLEST; double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0; uint32_t indexMaxDotProduct = 0;
// For each vertex of the mesh // For each vertex of the mesh
for (uint i=0; i<m_numberVertices; i++) { for (uint32_t i=0; i<m_numberVertices; i++) {
// Compute the dot product of the current vertex // Compute the dot product of the current vertex
double dotProduct = direction.dot(m_vertices[i]); double dotProduct = direction.dot(m_vertices[i]);
// If the current dot product is larger than the maximum one // If the current dot product is larger than the maximum one
if (dotProduct > maxDotProduct) { if (dotProduct > maxDotProduct) {
indexMaxDotProduct = i; indexMaxDotProduct = i;
maxDotProduct = dotProduct; maxDotProduct = dotProduct;
} }
} }
assert(maxDotProduct >= decimal(0.0)); assert(maxDotProduct >= float(0.0));
// Return the vertex with the largest dot product in the support direction // Return the vertex with the largest dot product in the support direction
return m_vertices[indexMaxDotProduct] * mScaling; return m_vertices[indexMaxDotProduct] * mScaling;
} }
} }
// Recompute the bounds of the mesh // Recompute the bounds of the mesh
void ConvexMeshShape::recalculateBounds() { void ConvexMeshShape::recalculateBounds() {
// TODO : Only works if the local origin is inside the mesh // TODO : Only works if the local origin is inside the mesh
// => Make it more robust (init with first vertex of mesh instead) // => Make it more robust (init with first vertex of mesh instead)
m_minBounds.setToZero(); m_minBounds.setToZero();
m_maxBounds.setToZero(); m_maxBounds.setToZero();
// For each vertex of the mesh // For each vertex of the mesh
for (uint i=0; i<m_numberVertices; i++) { for (uint32_t i=0; i<m_numberVertices; i++) {
if (m_vertices[i].x > m_maxBounds.x) m_maxBounds.x = m_vertices[i].x; if (m_vertices[i].x > m_maxBounds.x) m_maxBounds.x = m_vertices[i].x;
if (m_vertices[i].x < m_minBounds.x) m_minBounds.x = m_vertices[i].x; if (m_vertices[i].x < m_minBounds.x) m_minBounds.x = m_vertices[i].x;
if (m_vertices[i].y > m_maxBounds.y) m_maxBounds.y = m_vertices[i].y; if (m_vertices[i].y > m_maxBounds.y) m_maxBounds.y = m_vertices[i].y;
if (m_vertices[i].y < m_minBounds.y) m_minBounds.y = m_vertices[i].y; if (m_vertices[i].y < m_minBounds.y) m_minBounds.y = m_vertices[i].y;
if (m_vertices[i].z > m_maxBounds.z) m_maxBounds.z = m_vertices[i].z; if (m_vertices[i].z > m_maxBounds.z) m_maxBounds.z = m_vertices[i].z;
if (m_vertices[i].z < m_minBounds.z) m_minBounds.z = m_vertices[i].z; if (m_vertices[i].z < m_minBounds.z) m_minBounds.z = m_vertices[i].z;
} }
// Apply the local scaling factor // Apply the local scaling factor
m_maxBounds = m_maxBounds * mScaling; m_maxBounds = m_maxBounds * mScaling;
m_minBounds = m_minBounds * mScaling; m_minBounds = m_minBounds * mScaling;
// Add the object margin to the bounds // Add the object margin to the bounds
m_maxBounds += Vector3(mMargin, mMargin, mMargin); m_maxBounds += Vector3(mMargin, mMargin, mMargin);
m_minBounds -= Vector3(mMargin, mMargin, mMargin); m_minBounds -= Vector3(mMargin, mMargin, mMargin);
} }
// Raycast method with feedback information // Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo); ray, proxyShape, raycastInfo);
} }

247
ephysics/collision/shapes/ConvexMeshShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_MESH_SHAPE_H
#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -60,103 +39,103 @@ class CollisionWorld;
*/ */
class ConvexMeshShape : public ConvexShape { class ConvexMeshShape : public ConvexShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Array with the vertices of the mesh /// Array with the vertices of the mesh
std::vector<Vector3> m_vertices; std::vector<Vector3> m_vertices;
/// Number of vertices in the mesh /// Number of vertices in the mesh
uint m_numberVertices; uint32_t m_numberVertices;
/// Mesh minimum bounds in the three local x, y and z directions /// Mesh minimum bounds in the three local x, y and z directions
Vector3 m_minBounds; Vector3 m_minBounds;
/// Mesh maximum bounds in the three local x, y and z directions /// Mesh maximum bounds in the three local x, y and z directions
Vector3 m_maxBounds; Vector3 m_maxBounds;
/// True if the shape contains the edges of the convex mesh in order to /// True if the shape contains the edges of the convex mesh in order to
/// make the collision detection faster /// make the collision detection faster
bool m_isEdgesInformationUsed; bool m_isEdgesInformationUsed;
/// Adjacency list representing the edges of the mesh /// Adjacency list representing the edges of the mesh
std::map<uint, std::set<uint> > m_edgesAdjacencyList; std::map<uint32_t, std::set<uint32_t> > m_edgesAdjacencyList;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ConvexMeshShape(const ConvexMeshShape& shape); ConvexMeshShape(const ConvexMeshShape& shape);
/// Private assignment operator /// Private assignment operator
ConvexMeshShape& operator=(const ConvexMeshShape& shape); ConvexMeshShape& operator=(const ConvexMeshShape& shape);
/// Recompute the bounds of the mesh /// Recompute the bounds of the mesh
void recalculateBounds(); void recalculateBounds();
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return a local support point in a given direction without the object margin. /// Return a local support point in a given direction without the object margin.
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// 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;
/// Raycast method with feedback information /// 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) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor to initialize with an array of 3D vertices. /// Constructor to initialize with an array of 3D vertices.
ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride,
decimal margin = OBJECT_MARGIN); float margin = OBJECT_MARGIN);
/// Constructor to initialize with a triangle vertex array /// Constructor to initialize with a triangle vertex array
ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true, ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true,
decimal margin = OBJECT_MARGIN); float margin = OBJECT_MARGIN);
/// Constructor. /// Constructor.
ConvexMeshShape(decimal margin = OBJECT_MARGIN); ConvexMeshShape(float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~ConvexMeshShape(); virtual ~ConvexMeshShape();
/// Return the local bounds of the shape in x, y and z directions /// 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;
/// Return the local inertia tensor of the collision shape. /// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Add a vertex into the convex mesh /// Add a vertex int32_to the convex mesh
void addVertex(const Vector3& vertex); void addVertex(const Vector3& vertex);
/// Add an edge into the convex mesh by specifying the two vertex indices of the edge. /// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
void addEdge(uint v1, uint v2); void addEdge(uint32_t v1, uint32_t v2);
/// Return true if the edges information is used to speed up the collision detection /// Return true if the edges information is used to speed up the collision detection
bool isEdgesInformationUsed() const; bool isEdgesInformationUsed() const;
/// Set the variable to know if the edges information is used to speed up the /// Set the variable to know if the edges information is used to speed up the
/// collision detection /// collision detection
void setIsEdgesInformationUsed(bool isEdgesUsed); void setIsEdgesInformationUsed(bool isEdgesUsed);
}; };
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
inline void ConvexMeshShape::setLocalScaling(const Vector3& scaling) { inline void ConvexMeshShape::setLocalScaling(const Vector3& scaling) {
ConvexShape::setLocalScaling(scaling); ConvexShape::setLocalScaling(scaling);
recalculateBounds(); recalculateBounds();
} }
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t ConvexMeshShape::getSizeInBytes() const { inline size_t ConvexMeshShape::getSizeInBytes() const {
return sizeof(ConvexMeshShape); return sizeof(ConvexMeshShape);
} }
// Return the local bounds of the shape in x, y and z directions // Return the local bounds of the shape in x, y and z directions
@ -165,8 +144,8 @@ inline size_t ConvexMeshShape::getSizeInBytes() const {
* @param max The maximum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates
*/ */
inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
min = m_minBounds; min = m_minBounds;
max = m_maxBounds; max = m_maxBounds;
} }
// Return the local inertia tensor of the collision shape. // Return the local inertia tensor of the collision shape.
@ -174,63 +153,63 @@ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
/// of its bounding box. /// of its bounding box.
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass; float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = decimal(0.5) * (m_maxBounds - m_minBounds); Vector3 realExtent = float(0.5) * (m_maxBounds - m_minBounds);
assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0); assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
decimal xSquare = realExtent.x * realExtent.x; float xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y; float ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z; float zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0, 0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare)); 0.0, 0.0, factor * (xSquare + ySquare));
} }
// Add a vertex into the convex mesh // Add a vertex int32_to the convex mesh
/** /**
* @param vertex Vertex to be added * @param vertex Vertex to be added
*/ */
inline void ConvexMeshShape::addVertex(const Vector3& vertex) { inline void ConvexMeshShape::addVertex(const Vector3& vertex) {
// Add the vertex in to vertices array // Add the vertex in to vertices array
m_vertices.push_back(vertex); m_vertices.push_back(vertex);
m_numberVertices++; m_numberVertices++;
// Update the bounds of the mesh // Update the bounds of the mesh
if (vertex.x * mScaling.x > m_maxBounds.x) m_maxBounds.x = vertex.x * mScaling.x; if (vertex.x * mScaling.x > m_maxBounds.x) m_maxBounds.x = vertex.x * mScaling.x;
if (vertex.x * mScaling.x < m_minBounds.x) m_minBounds.x = vertex.x * mScaling.x; if (vertex.x * mScaling.x < m_minBounds.x) m_minBounds.x = vertex.x * mScaling.x;
if (vertex.y * mScaling.y > m_maxBounds.y) m_maxBounds.y = vertex.y * mScaling.y; if (vertex.y * mScaling.y > m_maxBounds.y) m_maxBounds.y = vertex.y * mScaling.y;
if (vertex.y * mScaling.y < m_minBounds.y) m_minBounds.y = vertex.y * mScaling.y; if (vertex.y * mScaling.y < m_minBounds.y) m_minBounds.y = vertex.y * mScaling.y;
if (vertex.z * mScaling.z > m_maxBounds.z) m_maxBounds.z = vertex.z * mScaling.z; if (vertex.z * mScaling.z > m_maxBounds.z) m_maxBounds.z = vertex.z * mScaling.z;
if (vertex.z * mScaling.z < m_minBounds.z) m_minBounds.z = vertex.z * mScaling.z; if (vertex.z * mScaling.z < m_minBounds.z) m_minBounds.z = vertex.z * mScaling.z;
} }
// Add an edge into the convex mesh by specifying the two vertex indices of the edge. // Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
/// Note that the vertex indices start at zero and need to correspond to the order of /// Note that the vertex indices start at zero and need to correspond to the order of
/// the vertices in the vertices array in the constructor or the order of the calls /// the vertices in the vertices array in the constructor or the order of the calls
/// of the addVertex() methods that you use to add vertices into the convex mesh. /// of the addVertex() methods that you use to add vertices int32_to the convex mesh.
/** /**
* @param v1 Index of the first vertex of the edge to add * @param v1 Index of the first vertex of the edge to add
* @param v2 Index of the second vertex of the edge to add * @param v2 Index of the second vertex of the edge to add
*/ */
inline void ConvexMeshShape::addEdge(uint v1, uint v2) { inline void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) {
// If the entry for vertex v1 does not exist in the adjacency list // If the entry for vertex v1 does not exist in the adjacency list
if (m_edgesAdjacencyList.count(v1) == 0) { if (m_edgesAdjacencyList.count(v1) == 0) {
m_edgesAdjacencyList.insert(std::make_pair(v1, std::set<uint>())); m_edgesAdjacencyList.insert(std::make_pair(v1, std::set<uint32_t>()));
} }
// If the entry for vertex v2 does not exist in the adjacency list // If the entry for vertex v2 does not exist in the adjacency list
if (m_edgesAdjacencyList.count(v2) == 0) { if (m_edgesAdjacencyList.count(v2) == 0) {
m_edgesAdjacencyList.insert(std::make_pair(v2, std::set<uint>())); m_edgesAdjacencyList.insert(std::make_pair(v2, std::set<uint32_t>()));
} }
// Add the edge in the adjacency list // Add the edge in the adjacency list
m_edgesAdjacencyList[v1].insert(v2); m_edgesAdjacencyList[v1].insert(v2);
m_edgesAdjacencyList[v2].insert(v1); m_edgesAdjacencyList[v2].insert(v1);
} }
// Return true if the edges information is used to speed up the collision detection // Return true if the edges information is used to speed up the collision detection
@ -238,28 +217,26 @@ inline void ConvexMeshShape::addEdge(uint v1, uint v2) {
* @return True if the edges information is used and false otherwise * @return True if the edges information is used and false otherwise
*/ */
inline bool ConvexMeshShape::isEdgesInformationUsed() const { inline bool ConvexMeshShape::isEdgesInformationUsed() const {
return m_isEdgesInformationUsed; return m_isEdgesInformationUsed;
} }
// Set the variable to know if the edges information is used to speed up the // Set the variable to know if the edges information is used to speed up the
// collision detection // collision detection
/** /**
* @param isEdgesUsed True if you want to use the edges information to speed up * @param isEdgesUsed True if you want to use the edges information to speed up
* the collision detection with the convex mesh shape * the collision detection with the convex mesh shape
*/ */
inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
m_isEdgesInformationUsed = isEdgesUsed; m_isEdgesInformationUsed = isEdgesUsed;
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
ProxyShape* proxyShape) const { ProxyShape* proxyShape) const {
// Use the GJK algorithm to test if the point is inside the convex mesh // Use the GJK algorithm to test if the point is inside the convex mesh
return proxyShape->mBody->mWorld.mCollisionDetection. return proxyShape->mBody->mWorld.mCollisionDetection.
mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
} }
} }
#endif

57
ephysics/collision/shapes/ConvexShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -31,8 +12,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
ConvexShape::ConvexShape(CollisionShapeType type, decimal margin) ConvexShape::ConvexShape(CollisionShapeType type, float margin)
: CollisionShape(type), mMargin(margin) { : CollisionShape(type), mMargin(margin) {
} }
@ -43,20 +24,20 @@ ConvexShape::~ConvexShape() {
// Return a local support point in a given direction with the object margin // Return a local support point in a given direction with the object margin
Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction, Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
// Get the support point without margin // Get the support point without margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
if (mMargin != decimal(0.0)) { if (mMargin != float(0.0)) {
// Add the margin to the support point // Add the margin to the support point
Vector3 unitVec(0.0, -1.0, 0.0); Vector3 unitVec(0.0, -1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit(); unitVec = direction.getUnit();
} }
supportPoint += unitVec * mMargin; supportPoint += unitVec * mMargin;
} }
return supportPoint; return supportPoint;
} }

100
ephysics/collision/shapes/ConvexShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SHAPE_H
#define REACTPHYSICS3D_CONVEX_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/CollisionShape.h> #include <ephysics/collision/shapes/CollisionShape.h>
@ -39,68 +18,67 @@ namespace reactphysics3d {
*/ */
class ConvexShape : public CollisionShape { class ConvexShape : public CollisionShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Margin used for the GJK collision detection algorithm /// Margin used for the GJK collision detection algorithm
decimal mMargin; float mMargin;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ConvexShape(const ConvexShape& shape); ConvexShape(const ConvexShape& shape);
/// Private assignment operator /// Private assignment operator
ConvexShape& operator=(const ConvexShape& shape); ConvexShape& operator=(const ConvexShape& shape);
// Return a local support point in a given direction with the object margin // Return a local support point in a given direction with the object margin
Vector3 getLocalSupportPointWithMargin(const Vector3& direction, Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const=0; void** cachedCollisionData) const=0;
/// Return true if a point is inside the collision shape /// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConvexShape(CollisionShapeType type, decimal margin); ConvexShape(CollisionShapeType type, float margin);
/// Destructor /// Destructor
virtual ~ConvexShape(); virtual ~ConvexShape();
/// Return the current object margin /// Return the current object margin
decimal getMargin() const; float getMargin() const;
/// Return true if the collision shape is convex, false if it is concave /// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const; virtual bool isConvex() const;
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class GJKAlgorithm; friend class GJKAlgorithm;
friend class EPAAlgorithm; friend class EPAAlgorithm;
}; };
/// Return true if the collision shape is convex, false if it is concave /// Return true if the collision shape is convex, false if it is concave
inline bool ConvexShape::isConvex() const { inline bool ConvexShape::isConvex() const {
return true; return true;
} }
// Return the current collision shape margin // Return the current collision shape margin
/** /**
* @return The margin (in meters) around the collision shape * @return The margin (in meters) around the collision shape
*/ */
inline decimal ConvexShape::getMargin() const { inline float ConvexShape::getMargin() const {
return mMargin; return mMargin;
} }
} }
#endif

313
ephysics/collision/shapes/CylinderShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/CylinderShape.h> #include <ephysics/collision/shapes/CylinderShape.h>
@ -36,11 +17,11 @@ using namespace reactphysics3d;
* @param height Height of the cylinder (in meters) * @param height Height of the cylinder (in meters)
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin) CylinderShape::CylinderShape(float radius, float height, float margin)
: ConvexShape(CYLINDER, margin), mRadius(radius), : ConvexShape(CYLINDER, margin), mRadius(radius),
mHalfHeight(height/decimal(2.0)) { mHalfHeight(height/float(2.0)) {
assert(radius > decimal(0.0)); assert(radius > float(0.0));
assert(height > decimal(0.0)); assert(height > float(0.0));
} }
// Destructor // Destructor
@ -50,24 +31,24 @@ CylinderShape::~CylinderShape() {
// Return a local support point in a given direction without the object margin // Return a local support point in a given direction without the object margin
Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction, Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
Vector3 supportPoint(0.0, 0.0, 0.0); Vector3 supportPoint(0.0, 0.0, 0.0);
decimal uDotv = direction.y; float uDotv = direction.y;
Vector3 w(direction.x, 0.0, direction.z); Vector3 w(direction.x, 0.0, direction.z);
decimal lengthW = sqrt(direction.x * direction.x + direction.z * direction.z); float lengthW = sqrt(direction.x * direction.x + direction.z * direction.z);
if (lengthW > MACHINE_EPSILON) { if (lengthW > MACHINE_EPSILON) {
if (uDotv < 0.0) supportPoint.y = -mHalfHeight; if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
else supportPoint.y = mHalfHeight; else supportPoint.y = mHalfHeight;
supportPoint += (mRadius / lengthW) * w; supportPoint += (mRadius / lengthW) * w;
} }
else { else {
if (uDotv < 0.0) supportPoint.y = -mHalfHeight; if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
else supportPoint.y = mHalfHeight; else supportPoint.y = mHalfHeight;
} }
return supportPoint; return supportPoint;
} }
// Raycast method with feedback information // Raycast method with feedback information
@ -75,162 +56,162 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
/// Morgan Kaufmann. /// Morgan Kaufmann.
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 n = ray.point2 - ray.point1; const Vector3 n = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.01); const float epsilon = float(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0)); Vector3 p(float(0), -mHalfHeight, float(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0)); Vector3 q(float(0), mHalfHeight, float(0));
Vector3 d = q - p; Vector3 d = q - p;
Vector3 m = ray.point1 - p; Vector3 m = ray.point1 - p;
decimal t; float t;
decimal mDotD = m.dot(d); float mDotD = m.dot(d);
decimal nDotD = n.dot(d); float nDotD = n.dot(d);
decimal dDotD = d.dot(d); float dDotD = d.dot(d);
// Test if the segment is outside the cylinder // Test if the segment is outside the cylinder
if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false; if (mDotD < float(0.0) && mDotD + nDotD < float(0.0)) return false;
if (mDotD > dDotD && mDotD + nDotD > dDotD) return false; if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
decimal nDotN = n.dot(n); float nDotN = n.dot(n);
decimal mDotN = m.dot(n); float mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD; float a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mRadius * mRadius; float k = m.dot(m) - mRadius * mRadius;
decimal c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the cylinder axis // If the ray is parallel to the cylinder axis
if (std::abs(a) < epsilon) { if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the cylinder, we return no hit // If the origin is outside the surface of the cylinder, we return no hit
if (c > decimal(0.0)) return false; if (c > float(0.0)) return false;
// Here we know that the segment intersect an endcap of the cylinder // Here we know that the segment int32_tersect an endcap of the cylinder
// If the ray intersects with the "p" endcap of the cylinder // If the ray int32_tersects with the "p" endcap of the cylinder
if (mDotD < decimal(0.0)) { if (mDotD < float(0.0)) {
t = -mDotN / nDotN; t = -mDotN / nDotN;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1), 0); Vector3 normalDirection(0, float(-1), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder
t = (nDotD - mDotN) / nDotN; t = (nDotD - mDotN) / nDotN;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0); Vector3 normalDirection(0, float(1.0), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
else { // If the origin is inside the cylinder, we return no hit else { // If the origin is inside the cylinder, we return no hit
return false; return false;
} }
} }
decimal b = dDotD * mDotN - nDotD * mDotD; float b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c; float discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit // If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < decimal(0.0)) return false; if (discriminant < float(0.0)) return false;
// Compute the smallest root (first intersection along the ray) // Compute the smallest root (first int32_tersection along the ray)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a; float t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the cylinder on "p" endcap side // If the int32_tersection is outside the cylinder on "p" endcap side
decimal value = mDotD + t * nDotD; float value = mDotD + t * nDotD;
if (value < decimal(0.0)) { if (value < float(0.0)) {
// If the ray is pointing away from the "p" endcap, we return no hit // If the ray is pointing away from the "p" endcap, we return no hit
if (nDotD <= decimal(0.0)) return false; if (nDotD <= float(0.0)) return false;
// Compute the intersection against the "p" endcap (intersection agains whole plane) // Compute the int32_tersection against the "p" endcap (int32_tersection agains whole plane)
t = -mDotD / nDotD; t = -mDotD / nDotD;
// Keep the intersection if the it is inside the cylinder radius // Keep the int32_tersection if the it is inside the cylinder radius
if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false; if (k + t * (float(2.0) * mDotN + t) > float(0.0)) return false;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1.0), 0); Vector3 normalDirection(0, float(-1.0), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side else if (value > dDotD) { // If the int32_tersection is outside the cylinder on the "q" side
// If the ray is pointing away from the "q" endcap, we return no hit // If the ray is pointing away from the "q" endcap, we return no hit
if (nDotD >= decimal(0.0)) return false; if (nDotD >= float(0.0)) return false;
// Compute the intersection against the "q" endcap (intersection against whole plane) // Compute the int32_tersection against the "q" endcap (int32_tersection against whole plane)
t = (dDotD - mDotD) / nDotD; t = (dDotD - mDotD) / nDotD;
// Keep the intersection if it is inside the cylinder radius // Keep the int32_tersection if it is inside the cylinder radius
if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) > if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) >
decimal(0.0)) return false; float(0.0)) return false;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0); Vector3 normalDirection(0, float(1.0), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
t = t0; t = t0;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p; Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d; Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)); Vector3 normalDirection = (localHitPoint - (p + w));
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }

156
ephysics/collision/shapes/CylinderShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CYLINDER_SHAPE_H
#define REACTPHYSICS3D_CYLINDER_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -51,91 +30,91 @@ namespace reactphysics3d {
*/ */
class CylinderShape : public ConvexShape { class CylinderShape : public ConvexShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Radius of the base /// Radius of the base
decimal mRadius; float mRadius;
/// Half height of the cylinder /// Half height of the cylinder
decimal mHalfHeight; float mHalfHeight;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
CylinderShape(const CylinderShape& shape); CylinderShape(const CylinderShape& shape);
/// Private assignment operator /// Private assignment operator
CylinderShape& operator=(const CylinderShape& shape); CylinderShape& operator=(const CylinderShape& shape);
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// 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;
/// Raycast method with feedback information /// 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) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN); CylinderShape(float radius, float height, float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~CylinderShape(); virtual ~CylinderShape();
/// Return the radius /// Return the radius
decimal getRadius() const; float getRadius() const;
/// Return the height /// Return the height
decimal getHeight() const; float getHeight() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions /// 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;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
}; };
// Return the radius // Return the radius
/** /**
* @return Radius of the cylinder (in meters) * @return Radius of the cylinder (in meters)
*/ */
inline decimal CylinderShape::getRadius() const { inline float CylinderShape::getRadius() const {
return mRadius; return mRadius;
} }
// Return the height // Return the height
/** /**
* @return Height of the cylinder (in meters) * @return Height of the cylinder (in meters)
*/ */
inline decimal CylinderShape::getHeight() const { inline float CylinderShape::getHeight() const {
return mHalfHeight + mHalfHeight; return mHalfHeight + mHalfHeight;
} }
// Set the scaling vector of the collision shape // Set the scaling vector of the collision shape
inline void CylinderShape::setLocalScaling(const Vector3& scaling) { inline void CylinderShape::setLocalScaling(const Vector3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mRadius = (mRadius / mScaling.x) * scaling.x; mRadius = (mRadius / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t CylinderShape::getSizeInBytes() const { inline size_t CylinderShape::getSizeInBytes() const {
return sizeof(CylinderShape); return sizeof(CylinderShape);
} }
// Return the local bounds of the shape in x, y and z directions // Return the local bounds of the shape in x, y and z directions
@ -145,38 +124,37 @@ inline size_t CylinderShape::getSizeInBytes() const {
*/ */
inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds // Maximum bounds
max.x = mRadius + mMargin; max.x = mRadius + mMargin;
max.y = mHalfHeight + mMargin; max.y = mHalfHeight + mMargin;
max.z = max.x; max.z = max.x;
// Minimum bounds // Minimum bounds
min.x = -max.x; min.x = -max.x;
min.y = -max.y; min.y = -max.y;
min.z = min.x; min.z = min.x;
} }
// Return the local inertia tensor of the cylinder // Return the local inertia tensor of the cylinder
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal height = decimal(2.0) * mHalfHeight; float height = float(2.0) * mHalfHeight;
decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height); float diag = (float(1.0) / float(12.0)) * mass * (3 * mRadius * mRadius + height * height);
tensor.setAllValues(diag, 0.0, 0.0, 0.0, tensor.setAllValues(diag, 0.0, 0.0, 0.0,
decimal(0.5) * mass * mRadius * mRadius, 0.0, float(0.5) * mass * mRadius * mRadius, 0.0,
0.0, 0.0, diag); 0.0, 0.0, diag);
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{ inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{
return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius && return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight); localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight);
} }
} }
#endif

313
ephysics/collision/shapes/HeightFieldShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/HeightFieldShape.h> #include <ephysics/collision/shapes/HeightFieldShape.h>
@ -35,43 +16,43 @@ using namespace reactphysics3d;
* @param minHeight Minimum height value of the height field * @param minHeight Minimum height value of the height field
* @param maxHeight Maximum height value of the height field * @param maxHeight Maximum height value of the height field
* @param heightFieldData Pointer to the first height value data (note that values are shared and not copied) * @param heightFieldData Pointer to the first height value data (note that values are shared and not copied)
* @param dataType Data type for the height values (int, float, double) * @param dataType Data type for the height values (int32_t, float, double)
* @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z) * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z)
* @param integerHeightScale Scaling factor used to scale the height values (only when height values type is integer) * @param int32_tegerHeightScale Scaling factor used to scale the height values (only when height values type is int32_teger)
*/ */
HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, HeightFieldShape::HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight,
const void* heightFieldData, HeightDataType dataType, int upAxis, const void* heightFieldData, HeightDataType dataType, int32_t upAxis,
decimal integerHeightScale) float int32_tegerHeightScale)
: ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), : ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight), mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight),
mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale), mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(int32_tegerHeightScale),
mHeightDataType(dataType) { mHeightDataType(dataType) {
assert(nbGridColumns >= 2); assert(nbGridColumns >= 2);
assert(nbGridRows >= 2); assert(nbGridRows >= 2);
assert(mWidth >= 1); assert(mWidth >= 1);
assert(mLength >= 1); assert(mLength >= 1);
assert(minHeight <= maxHeight); assert(minHeight <= maxHeight);
assert(upAxis == 0 || upAxis == 1 || upAxis == 2); assert(upAxis == 0 || upAxis == 1 || upAxis == 2);
mHeightFieldData = heightFieldData; mHeightFieldData = heightFieldData;
decimal halfHeight = (mMaxHeight - mMinHeight) * decimal(0.5); float halfHeight = (mMaxHeight - mMinHeight) * float(0.5);
assert(halfHeight >= 0); assert(halfHeight >= 0);
// Compute the local AABB of the height field // Compute the local AABB of the height field
if (mUpAxis == 0) { if (mUpAxis == 0) {
mAABB.setMin(Vector3(-halfHeight, -mWidth * decimal(0.5), -mLength * decimal(0.5))); mAABB.setMin(Vector3(-halfHeight, -mWidth * float(0.5), -mLength * float(0.5)));
mAABB.setMax(Vector3(halfHeight, mWidth * decimal(0.5), mLength* decimal(0.5))); mAABB.setMax(Vector3(halfHeight, mWidth * float(0.5), mLength* float(0.5)));
} }
else if (mUpAxis == 1) { else if (mUpAxis == 1) {
mAABB.setMin(Vector3(-mWidth * decimal(0.5), -halfHeight, -mLength * decimal(0.5))); mAABB.setMin(Vector3(-mWidth * float(0.5), -halfHeight, -mLength * float(0.5)));
mAABB.setMax(Vector3(mWidth * decimal(0.5), halfHeight, mLength * decimal(0.5))); mAABB.setMax(Vector3(mWidth * float(0.5), halfHeight, mLength * float(0.5)));
} }
else if (mUpAxis == 2) { else if (mUpAxis == 2) {
mAABB.setMin(Vector3(-mWidth * decimal(0.5), -mLength * decimal(0.5), -halfHeight)); mAABB.setMin(Vector3(-mWidth * float(0.5), -mLength * float(0.5), -halfHeight));
mAABB.setMax(Vector3(mWidth * decimal(0.5), mLength * decimal(0.5), halfHeight)); mAABB.setMax(Vector3(mWidth * float(0.5), mLength * float(0.5), halfHeight));
} }
} }
// Destructor // Destructor
@ -86,8 +67,8 @@ HeightFieldShape::~HeightFieldShape() {
* @param max The maximum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates
*/ */
void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const {
min = mAABB.getMin() * mScaling; min = mAABB.getMin() * mScaling;
max = mAABB.getMax() * mScaling; max = mAABB.getMax() * mScaling;
} }
// Test collision with the triangles of the height field shape. The idea is to use the AABB // Test collision with the triangles of the height field shape. The idea is to use the AABB
@ -97,35 +78,35 @@ void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const {
void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
// Compute the non-scaled AABB // Compute the non-scaled AABB
Vector3 inverseScaling(decimal(1.0) / mScaling.x, decimal(1.0) / mScaling.y, decimal(1.0) / mScaling.z); Vector3 inverseScaling(float(1.0) / mScaling.x, float(1.0) / mScaling.y, float(1.0) / mScaling.z);
AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling); AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
// Compute the integer grid coordinates inside the area we need to test for collision // Compute the int32_teger grid coordinates inside the area we need to test for collision
int minGridCoords[3]; int32_t minGridCoords[3];
int maxGridCoords[3]; int32_t maxGridCoords[3];
computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb); computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb);
// Compute the starting and ending coords of the sub-grid according to the up axis // Compute the starting and ending coords of the sub-grid according to the up axis
int iMin = 0; int32_t iMin = 0;
int iMax = 0; int32_t iMax = 0;
int jMin = 0; int32_t jMin = 0;
int jMax = 0; int32_t jMax = 0;
switch(mUpAxis) { switch(mUpAxis) {
case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1); case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1); iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[2], 0, mNbRows - 1); jMin = clamp(minGridCoords[2], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[2], 0, mNbRows - 1); jMax = clamp(maxGridCoords[2], 0, mNbRows - 1);
break; break;
case 1 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1); case 1 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1); iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[2], 0, mNbRows - 1); jMin = clamp(minGridCoords[2], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[2], 0, mNbRows - 1); jMax = clamp(maxGridCoords[2], 0, mNbRows - 1);
break; break;
case 2 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1); case 2 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1); iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[1], 0, mNbRows - 1); jMin = clamp(minGridCoords[1], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[1], 0, mNbRows - 1); jMax = clamp(maxGridCoords[1], 0, mNbRows - 1);
break; break;
} }
assert(iMin >= 0 && iMin < mNbColumns); assert(iMin >= 0 && iMin < mNbColumns);
@ -134,60 +115,60 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
assert(jMax >= 0 && jMax < mNbRows); assert(jMax >= 0 && jMax < mNbRows);
// For each sub-grid points (except the last ones one each dimension) // For each sub-grid points (except the last ones one each dimension)
for (int i = iMin; i < iMax; i++) { for (int32_t i = iMin; i < iMax; i++) {
for (int j = jMin; j < jMax; j++) { for (int32_t j = jMin; j < jMax; j++) {
// Compute the four point of the current quad // Compute the four point of the current quad
Vector3 p1 = getVertexAt(i, j); Vector3 p1 = getVertexAt(i, j);
Vector3 p2 = getVertexAt(i, j + 1); Vector3 p2 = getVertexAt(i, j + 1);
Vector3 p3 = getVertexAt(i + 1, j); Vector3 p3 = getVertexAt(i + 1, j);
Vector3 p4 = getVertexAt(i + 1, j + 1); Vector3 p4 = getVertexAt(i + 1, j + 1);
// Generate the first triangle for the current grid rectangle // Generate the first triangle for the current grid rectangle
Vector3 trianglePoints[3] = {p1, p2, p3}; Vector3 trianglePoints[3] = {p1, p2, p3};
// Test collision against the first triangle // Test collision against the first triangle
callback.testTriangle(trianglePoints); callback.testTriangle(trianglePoints);
// Generate the second triangle for the current grid rectangle // Generate the second triangle for the current grid rectangle
trianglePoints[0] = p3; trianglePoints[0] = p3;
trianglePoints[1] = p2; trianglePoints[1] = p2;
trianglePoints[2] = p4; trianglePoints[2] = p4;
// Test collision against the second triangle // Test collision against the second triangle
callback.testTriangle(trianglePoints); callback.testTriangle(trianglePoints);
} }
} }
} }
// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and // Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and
// the AABB to collide // the AABB to collide
void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const { void HeightFieldShape::computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const {
// Clamp the min/max coords of the AABB to collide inside the height field AABB // Clamp the min/max coords of the AABB to collide inside the height field AABB
Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin()); Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin());
minPoint = Vector3::min(minPoint, mAABB.getMax()); minPoint = Vector3::min(minPoint, mAABB.getMax());
Vector3 maxPoint = Vector3::min(aabbToCollide.getMax(), mAABB.getMax()); Vector3 maxPoint = Vector3::min(aabbToCollide.getMax(), mAABB.getMax());
maxPoint = Vector3::max(maxPoint, mAABB.getMin()); maxPoint = Vector3::max(maxPoint, mAABB.getMin());
// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints] // Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints]
// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2] // and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2]
// and [-mLength/2 ... mLength/2] // and [-mLength/2 ... mLength/2]
const Vector3 translateVec = mAABB.getExtent() * decimal(0.5); const Vector3 translateVec = mAABB.getExtent() * float(0.5);
minPoint += translateVec; minPoint += translateVec;
maxPoint += translateVec; maxPoint += translateVec;
// Convert the floating min/max coords of the AABB into closest integer // Convert the floating min/max coords of the AABB int32_to closest int32_teger
// grid values (note that we use the closest grid coordinate that is out // grid values (note that we use the closest grid coordinate that is out
// of the AABB) // of the AABB)
minCoords[0] = computeIntegerGridValue(minPoint.x) - 1; minCoords[0] = computeIntegerGridValue(minPoint.x) - 1;
minCoords[1] = computeIntegerGridValue(minPoint.y) - 1; minCoords[1] = computeIntegerGridValue(minPoint.y) - 1;
minCoords[2] = computeIntegerGridValue(minPoint.z) - 1; minCoords[2] = computeIntegerGridValue(minPoint.z) - 1;
maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1; maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1;
maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1; maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1;
maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1; maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1;
} }
// Raycast method with feedback information // Raycast method with feedback information
@ -195,73 +176,73 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor
/// the ray hits many triangles. /// the ray hits many triangles.
bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// TODO : Implement raycasting without using an AABB for the ray // TODO : Implement raycasting without using an AABB for the ray
// but using a dynamic AABB tree or octree instead // but using a dynamic AABB tree or octree instead
PROFILE("HeightFieldShape::raycast()"); PROFILE("HeightFieldShape::raycast()");
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
// Compute the AABB for the ray // Compute the AABB for the ray
const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd)); const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd));
testAllTriangles(triangleCallback, rayAABB); testAllTriangles(triangleCallback, rayAABB);
return triangleCallback.getIsHit(); return triangleCallback.getIsHit();
} }
// Return the vertex (local-coordinates) of the height field at a given (x,y) position // Return the vertex (local-coordinates) of the height field at a given (x,y) position
Vector3 HeightFieldShape::getVertexAt(int x, int y) const { Vector3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const {
// Get the height value // Get the height value
const decimal height = getHeightAt(x, y); const float height = getHeightAt(x, y);
// Height values origin // Height values origin
const decimal heightOrigin = -(mMaxHeight - mMinHeight) * decimal(0.5) - mMinHeight; const float heightOrigin = -(mMaxHeight - mMinHeight) * float(0.5) - mMinHeight;
Vector3 vertex; Vector3 vertex;
switch (mUpAxis) { switch (mUpAxis) {
case 0: vertex = Vector3(heightOrigin + height, -mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y); case 0: vertex = Vector3(heightOrigin + height, -mWidth * float(0.5) + x, -mLength * float(0.5) + y);
break; break;
case 1: vertex = Vector3(-mWidth * decimal(0.5) + x, heightOrigin + height, -mLength * decimal(0.5) + y); case 1: vertex = Vector3(-mWidth * float(0.5) + x, heightOrigin + height, -mLength * float(0.5) + y);
break; break;
case 2: vertex = Vector3(-mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y, heightOrigin + height); case 2: vertex = Vector3(-mWidth * float(0.5) + x, -mLength * float(0.5) + y, heightOrigin + height);
break; break;
default: assert(false); default: assert(false);
} }
assert(mAABB.contains(vertex)); assert(mAABB.contains(vertex));
return vertex * mScaling; return vertex * mScaling;
} }
// Raycast test between a ray and a triangle of the heightfield // Raycast test between a ray and a triangle of the heightfield
void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) { void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape // Create a triangle collision shape
decimal margin = mHeightFieldShape.getTriangleMargin(); float margin = mHeightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
// Ray casting test against the collision shape // Ray casting test against the collision shape
RaycastInfo raycastInfo; RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape);
// If the ray hit the collision shape // If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) { if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {
assert(raycastInfo.hitFraction >= decimal(0.0)); assert(raycastInfo.hitFraction >= float(0.0));
m_raycastInfo.body = raycastInfo.body; m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape; m_raycastInfo.proxyShape = raycastInfo.proxyShape;
m_raycastInfo.hitFraction = raycastInfo.hitFraction; m_raycastInfo.hitFraction = raycastInfo.hitFraction;
m_raycastInfo.worldPoint = raycastInfo.worldPoint; m_raycastInfo.worldPoint = raycastInfo.worldPoint;
m_raycastInfo.worldNormal = raycastInfo.worldNormal; m_raycastInfo.worldNormal = raycastInfo.worldNormal;
m_raycastInfo.meshSubpart = -1; m_raycastInfo.meshSubpart = -1;
m_raycastInfo.triangleIndex = -1; m_raycastInfo.triangleIndex = -1;
mSmallestHitFraction = raycastInfo.hitFraction; mSmallestHitFraction = raycastInfo.hitFraction;
mIsHit = true; mIsHit = true;
} }
} }

276
ephysics/collision/shapes/HeightFieldShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_HEIGHTFIELD_SHAPE_H
#define REACTPHYSICS3D_HEIGHTFIELD_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConcaveShape.h> #include <ephysics/collision/shapes/ConcaveShape.h>
@ -41,30 +20,30 @@ class HeightFieldShape;
*/ */
class TriangleOverlapCallback : public TriangleCallback { class TriangleOverlapCallback : public TriangleCallback {
protected: protected:
const Ray& m_ray; const Ray& m_ray;
ProxyShape* m_proxyShape; ProxyShape* m_proxyShape;
RaycastInfo& m_raycastInfo; RaycastInfo& m_raycastInfo;
bool mIsHit; bool mIsHit;
decimal mSmallestHitFraction; float mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape; const HeightFieldShape& mHeightFieldShape;
public: public:
// Constructor // Constructor
TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo, TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
const HeightFieldShape& heightFieldShape) const HeightFieldShape& heightFieldShape)
: m_ray(ray), m_proxyShape(proxyShape), m_raycastInfo(raycastInfo), : m_ray(ray), m_proxyShape(proxyShape), m_raycastInfo(raycastInfo),
mHeightFieldShape (heightFieldShape) { mHeightFieldShape (heightFieldShape) {
mIsHit = false; mIsHit = false;
mSmallestHitFraction = m_ray.maxFraction; mSmallestHitFraction = m_ray.maxFraction;
} }
bool getIsHit() const {return mIsHit;} bool getIsHit() const {return mIsHit;}
/// Raycast test between a ray and a triangle of the heightfield /// Raycast test between a ray and a triangle of the heightfield
virtual void testTriangle(const Vector3* trianglePoints); virtual void testTriangle(const Vector3* trianglePoints);
}; };
@ -72,8 +51,8 @@ class TriangleOverlapCallback : public TriangleCallback {
/** /**
* This class represents a static height field that can be used to represent * This class represents a static height field that can be used to represent
* a terrain. The height field is made of a grid with rows and columns with a * a terrain. The height field is made of a grid with rows and columns with a
* height value at each grid point. Note that the height values are not copied into the shape * height value at each grid point. Note that the height values are not copied int32_to the shape
* but are shared instead. The height values can be of type integer, float or double. * but are shared instead. The height values can be of type int32_teger, float or double.
* When creating a HeightFieldShape, you need to specify the minimum and maximum height value of * When creating a HeightFieldShape, you need to specify the minimum and maximum height value of
* your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means * your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means
* that for instance, if the minimum height value is -200 and the maximum value is 400, the final * that for instance, if the minimum height value is -200 and the maximum value is 400, the final
@ -81,177 +60,176 @@ class TriangleOverlapCallback : public TriangleCallback {
*/ */
class HeightFieldShape : public ConcaveShape { class HeightFieldShape : public ConcaveShape {
public: public:
/// Data type for the height data of the height field /// Data type for the height data of the height field
enum HeightDataType {HEIGHT_FLOAT_TYPE, HEIGHT_DOUBLE_TYPE, HEIGHT_INT_TYPE}; enum HeightDataType {HEIGHT_FLOAT_TYPE, HEIGHT_DOUBLE_TYPE, HEIGHT_INT_TYPE};
protected: protected:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Number of columns in the grid of the height field /// Number of columns in the grid of the height field
int mNbColumns; int32_t mNbColumns;
/// Number of rows in the grid of the height field /// Number of rows in the grid of the height field
int mNbRows; int32_t mNbRows;
/// Height field width /// Height field width
decimal mWidth; float mWidth;
/// Height field length /// Height field length
decimal mLength; float mLength;
/// Minimum height of the height field /// Minimum height of the height field
decimal mMinHeight; float mMinHeight;
/// Maximum height of the height field /// Maximum height of the height field
decimal mMaxHeight; float mMaxHeight;
/// Up axis direction (0 => x, 1 => y, 2 => z) /// Up axis direction (0 => x, 1 => y, 2 => z)
int mUpAxis; int32_t mUpAxis;
/// Height values scale for height field with integer height values /// Height values scale for height field with int32_teger height values
decimal mIntegerHeightScale; float mIntegerHeightScale;
/// Data type of the height values /// Data type of the height values
HeightDataType mHeightDataType; HeightDataType mHeightDataType;
/// Array of data with all the height values of the height field /// Array of data with all the height values of the height field
const void* mHeightFieldData; const void* mHeightFieldData;
/// Local AABB of the height field (without scaling) /// Local AABB of the height field (without scaling)
AABB mAABB; AABB mAABB;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
HeightFieldShape(const HeightFieldShape& shape); HeightFieldShape(const HeightFieldShape& shape);
/// Private assignment operator /// Private assignment operator
HeightFieldShape& operator=(const HeightFieldShape& shape); HeightFieldShape& operator=(const HeightFieldShape& shape);
/// Raycast method with feedback information /// 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) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Insert all the triangles into the dynamic AABB tree /// Insert all the triangles int32_to the dynamic AABB tree
void initBVHTree(); void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle. /// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex,
Vector3* outTriangleVertices) const; Vector3* outTriangleVertices) const;
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position /// Return the vertex (local-coordinates) of the height field at a given (x,y) position
Vector3 getVertexAt(int x, int y) const; Vector3 getVertexAt(int32_t x, int32_t y) const;
/// Return the height of a given (x,y) point in the height field /// Return the height of a given (x,y) point in the height field
decimal getHeightAt(int x, int y) const; float getHeightAt(int32_t x, int32_t y) const;
/// Return the closest inside integer grid value of a given floating grid value /// Return the closest inside int32_teger grid value of a given floating grid value
int computeIntegerGridValue(decimal value) const; int32_t computeIntegerGridValue(float value) const;
/// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide /// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and the AABB to collide
void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const; void computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const;
public: public:
/// Constructor /// Constructor
HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight,
const void* heightFieldData, HeightDataType dataType, const void* heightFieldData, HeightDataType dataType,
int upAxis = 1, decimal integerHeightScale = 1.0f); int32_t upAxis = 1, float int32_tegerHeightScale = 1.0f);
/// Destructor /// Destructor
~HeightFieldShape(); ~HeightFieldShape();
/// Return the number of rows in the height field /// Return the number of rows in the height field
int getNbRows() const; int32_t getNbRows() const;
/// Return the number of columns in the height field /// Return the number of columns in the height field
int getNbColumns() const; int32_t getNbColumns() const;
/// Return the type of height value in the height field /// Return the type of height value in the height field
HeightDataType getHeightDataType() const; HeightDataType getHeightDataType() const;
/// Return the local bounds of the shape in x, y and z directions. /// 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;
/// Set the local scaling vector of the collision shape /// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB /// 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;
// ---------- Friendship ----------- // // ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback; friend class ConvexTriangleAABBOverlapCallback;
friend class ConcaveMeshRaycastCallback; friend class ConcaveMeshRaycastCallback;
}; };
// Return the number of rows in the height field // Return the number of rows in the height field
inline int HeightFieldShape::getNbRows() const { inline int32_t HeightFieldShape::getNbRows() const {
return mNbRows; return mNbRows;
} }
// Return the number of columns in the height field // Return the number of columns in the height field
inline int HeightFieldShape::getNbColumns() const { inline int32_t HeightFieldShape::getNbColumns() const {
return mNbColumns; return mNbColumns;
} }
// Return the type of height value in the height field // Return the type of height value in the height field
inline HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const { inline HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const {
return mHeightDataType; return mHeightDataType;
} }
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t HeightFieldShape::getSizeInBytes() const { inline size_t HeightFieldShape::getSizeInBytes() const {
return sizeof(HeightFieldShape); return sizeof(HeightFieldShape);
} }
// Set the local scaling vector of the collision shape // Set the local scaling vector of the collision shape
inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) { inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) {
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
// Return the height of a given (x,y) point in the height field // Return the height of a given (x,y) point in the height field
inline decimal HeightFieldShape::getHeightAt(int x, int y) const { inline float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const {
switch(mHeightDataType) { switch(mHeightDataType) {
case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x]; case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x]; case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale; case HEIGHT_INT_TYPE : return ((int32_t*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale;
default: assert(false); return 0; default: assert(false); return 0;
} }
} }
// Return the closest inside integer grid value of a given floating grid value // Return the closest inside int32_teger grid value of a given floating grid value
inline int HeightFieldShape::computeIntegerGridValue(decimal value) const { inline int32_t HeightFieldShape::computeIntegerGridValue(float value) const {
return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5); return (value < float(0.0)) ? value - float(0.5) : value + float(0.5);
} }
// Return the local inertia tensor // Return the local inertia tensor
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
// Default inertia tensor // Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh. // Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore, // However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used. // the inertia tensor is not used.
tensor.setAllValues(mass, 0, 0, tensor.setAllValues(mass, 0, 0,
0, mass, 0, 0, mass, 0,
0, 0, mass); 0, 0, mass);
} }
} }
#endif

91
ephysics/collision/shapes/SphereShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/SphereShape.h> #include <ephysics/collision/shapes/SphereShape.h>
@ -35,8 +16,8 @@ using namespace reactphysics3d;
/** /**
* @param radius Radius of the sphere (in meters) * @param radius Radius of the sphere (in meters)
*/ */
SphereShape::SphereShape(decimal radius) : ConvexShape(SPHERE, radius) { SphereShape::SphereShape(float radius) : ConvexShape(SPHERE, radius) {
assert(radius > decimal(0.0)); assert(radius > float(0.0));
} }
// Destructor // Destructor
@ -47,45 +28,45 @@ SphereShape::~SphereShape() {
// Raycast method with feedback information // Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 m = ray.point1; const Vector3 m = ray.point1;
decimal c = m.dot(m) - mMargin * mMargin; float c = m.dot(m) - mMargin * mMargin;
// If the origin of the ray is inside the sphere, we return no intersection // If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < decimal(0.0)) return false; if (c < float(0.0)) return false;
const Vector3 rayDirection = ray.point2 - ray.point1; const Vector3 rayDirection = ray.point2 - ray.point1;
decimal b = m.dot(rayDirection); float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray // If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no intersection // is pointing away from the sphere, there is no int32_tersection
if (b > decimal(0.0)) return false; if (b > float(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare(); float raySquareLength = rayDirection.lengthSquare();
// Compute the discriminant of the quadratic equation // Compute the discriminant of the quadratic equation
decimal discriminant = b * b - raySquareLength * c; float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no intersection // If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false; if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
decimal t = -b - std::sqrt(discriminant); float t = -b - std::sqrt(discriminant);
assert(t >= decimal(0.0)); assert(t >= float(0.0));
// If the hit point is withing the segment ray fraction // If the hit point is withing the segment ray fraction
if (t < ray.maxFraction * raySquareLength) { if (t < ray.maxFraction * raySquareLength) {
// Compute the intersection information // Compute the int32_tersection information
t /= raySquareLength; t /= raySquareLength;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = ray.point1 + t * rayDirection; raycastInfo.worldPoint = ray.point1 + t * rayDirection;
raycastInfo.worldNormal = raycastInfo.worldPoint; raycastInfo.worldNormal = raycastInfo.worldPoint;
return true; return true;
} }
return false; return false;
} }

157
ephysics/collision/shapes/SphereShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SHAPE_H
#define REACTPHYSICS3D_SPHERE_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -44,85 +23,85 @@ namespace reactphysics3d {
*/ */
class SphereShape : public ConvexShape { class SphereShape : public ConvexShape {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
SphereShape(const SphereShape& shape); SphereShape(const SphereShape& shape);
/// Private assignment operator /// Private assignment operator
SphereShape& operator=(const SphereShape& shape); SphereShape& operator=(const SphereShape& shape);
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// 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;
/// Raycast method with feedback information /// 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) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
SphereShape(decimal radius); SphereShape(float radius);
/// Destructor /// Destructor
virtual ~SphereShape(); virtual ~SphereShape();
/// Return the radius of the sphere /// Return the radius of the sphere
decimal getRadius() const; float getRadius() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions. /// 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;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Update the AABB of a body using its collision shape /// Update the AABB of a body using its collision shape
virtual void computeAABB(AABB& aabb, const Transform& transform) const; virtual void computeAABB(AABB& aabb, const Transform& transform) const;
}; };
// Get the radius of the sphere // Get the radius of the sphere
/** /**
* @return Radius of the sphere (in meters) * @return Radius of the sphere (in meters)
*/ */
inline decimal SphereShape::getRadius() const { inline float SphereShape::getRadius() const {
return mMargin; return mMargin;
} }
// Set the scaling vector of the collision shape // Set the scaling vector of the collision shape
inline void SphereShape::setLocalScaling(const Vector3& scaling) { inline void SphereShape::setLocalScaling(const Vector3& scaling) {
mMargin = (mMargin / mScaling.x) * scaling.x; mMargin = (mMargin / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t SphereShape::getSizeInBytes() const { inline size_t SphereShape::getSizeInBytes() const {
return sizeof(SphereShape); return sizeof(SphereShape);
} }
// Return a local support point in a given direction without the object margin // Return a local support point in a given direction without the object margin
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction, inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
// Return the center of the sphere (the radius is taken into account in the object margin) // Return the center of the sphere (the radius is taken int32_to account in the object margin)
return Vector3(0.0, 0.0, 0.0); return Vector3(0.0, 0.0, 0.0);
} }
// Return the local bounds of the shape in x, y and z directions. // Return the local bounds of the shape in x, y and z directions.
@ -133,51 +112,49 @@ inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& dir
*/ */
inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds // Maximum bounds
max.x = mMargin; max.x = mMargin;
max.y = mMargin; max.y = mMargin;
max.z = mMargin; max.z = mMargin;
// Minimum bounds // Minimum bounds
min.x = -mMargin; min.x = -mMargin;
min.y = min.x; min.y = min.x;
min.z = min.x; min.z = min.x;
} }
// Return the local inertia tensor of the sphere // Return the local inertia tensor of the sphere
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal diag = decimal(0.4) * mass * mMargin * mMargin; float diag = float(0.4) * mass * mMargin * mMargin;
tensor.setAllValues(diag, 0.0, 0.0, tensor.setAllValues(diag, 0.0, 0.0,
0.0, diag, 0.0, 0.0, diag, 0.0,
0.0, 0.0, diag); 0.0, 0.0, diag);
} }
// Update the AABB of a body using its collision shape // Update the AABB of a body using its collision shape
/** /**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates * computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape * @param transform Transform used to compute the AABB of the collision shape
*/ */
inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const { inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const {
// Get the local extents in x,y and z direction // Get the local extents in x,y and z direction
Vector3 extents(mMargin, mMargin, mMargin); Vector3 extents(mMargin, mMargin, mMargin);
// Update the AABB with the new minimum and maximum coordinates // Update the AABB with the new minimum and maximum coordinates
aabb.setMin(transform.getPosition() - extents); aabb.setMin(transform.getPosition() - extents);
aabb.setMax(transform.getPosition() + extents); aabb.setMax(transform.getPosition() + extents);
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.lengthSquare() < mMargin * mMargin); return (localPoint.lengthSquare() < mMargin * mMargin);
} }
} }
#endif

151
ephysics/collision/shapes/TriangleShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/collision/shapes/TriangleShape.h> #include <ephysics/collision/shapes/TriangleShape.h>
@ -39,12 +20,12 @@ using namespace reactphysics3d;
* @param point3 Third point of the triangle * @param point3 Third point of the triangle
* @param margin The collision margin (in meters) around the collision shape * @param margin The collision margin (in meters) around the collision shape
*/ */
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin) TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, float margin)
: ConvexShape(TRIANGLE, margin) { : ConvexShape(TRIANGLE, margin) {
mPoints[0] = point1; mPoints[0] = point1;
mPoints[1] = point2; mPoints[1] = point2;
mPoints[2] = point3; mPoints[2] = point3;
m_raycastTestType = FRONT; m_raycastTestType = FRONT;
} }
// Destructor // Destructor
@ -57,71 +38,71 @@ TriangleShape::~TriangleShape() {
/// Real-time Collision Detection by Christer Ericson. /// Real-time Collision Detection by Christer Ericson.
bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
PROFILE("TriangleShape::raycast()"); PROFILE("TriangleShape::raycast()");
const Vector3 pq = ray.point2 - ray.point1; const Vector3 pq = ray.point2 - ray.point1;
const Vector3 pa = mPoints[0] - ray.point1; const Vector3 pa = mPoints[0] - ray.point1;
const Vector3 pb = mPoints[1] - ray.point1; const Vector3 pb = mPoints[1] - ray.point1;
const Vector3 pc = mPoints[2] - ray.point1; const Vector3 pc = mPoints[2] - ray.point1;
// Test if the line PQ is inside the eges BC, CA and AB. We use the triple // Test if the line PQ is inside the eges BC, CA and AB. We use the triple
// product for this test. // product for this test.
const Vector3 m = pq.cross(pc); const Vector3 m = pq.cross(pc);
decimal u = pb.dot(m); float u = pb.dot(m);
if (m_raycastTestType == FRONT) { if (m_raycastTestType == FRONT) {
if (u < decimal(0.0)) return false; if (u < float(0.0)) return false;
} }
else if (m_raycastTestType == BACK) { else if (m_raycastTestType == BACK) {
if (u > decimal(0.0)) return false; if (u > float(0.0)) return false;
} }
decimal v = -pa.dot(m); float v = -pa.dot(m);
if (m_raycastTestType == FRONT) { if (m_raycastTestType == FRONT) {
if (v < decimal(0.0)) return false; if (v < float(0.0)) return false;
} }
else if (m_raycastTestType == BACK) { else if (m_raycastTestType == BACK) {
if (v > decimal(0.0)) return false; if (v > float(0.0)) return false;
} }
else if (m_raycastTestType == FRONT_AND_BACK) { else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, v)) return false; if (!sameSign(u, v)) return false;
} }
decimal w = pa.dot(pq.cross(pb)); float w = pa.dot(pq.cross(pb));
if (m_raycastTestType == FRONT) { if (m_raycastTestType == FRONT) {
if (w < decimal(0.0)) return false; if (w < float(0.0)) return false;
} }
else if (m_raycastTestType == BACK) { else if (m_raycastTestType == BACK) {
if (w > decimal(0.0)) return false; if (w > float(0.0)) return false;
} }
else if (m_raycastTestType == FRONT_AND_BACK) { else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, w)) return false; if (!sameSign(u, w)) return false;
} }
// If the line PQ is in the triangle plane (case where u=v=w=0) // If the line PQ is in the triangle plane (case where u=v=w=0)
if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false; if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false;
// Compute the barycentric coordinates (u, v, w) to determine the // Compute the barycentric coordinates (u, v, w) to determine the
// intersection point R, R = u * a + v * b + w * c // int32_tersection point R, R = u * a + v * b + w * c
decimal denom = decimal(1.0) / (u + v + w); float denom = float(1.0) / (u + v + w);
u *= denom; u *= denom;
v *= denom; v *= denom;
w *= denom; w *= denom;
// Compute the local hit point using the barycentric coordinates // Compute the local hit point using the barycentric coordinates
const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
const decimal hitFraction = (localHitPoint - ray.point1).length() / pq.length(); const float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false; if (hitFraction < float(0.0) || hitFraction > ray.maxFraction) return false;
Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal; if (localHitNormal.dot(pq) > float(0.0)) localHitNormal = -localHitNormal;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldNormal = localHitNormal; raycastInfo.worldNormal = localHitNormal;
return true; return true;
} }

201
ephysics/collision/shapes/TriangleShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SHAPE_H
#define REACTPHYSICS3D_TRIANGLE_SHAPE_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -36,14 +15,14 @@ namespace reactphysics3d {
/// Raycast test side for the triangle /// Raycast test side for the triangle
enum TriangleRaycastSide { enum TriangleRaycastSide {
/// Raycast against front triangle /// Raycast against front triangle
FRONT, FRONT,
/// Raycast against back triangle /// Raycast against back triangle
BACK, BACK,
/// Raycast against front and back triangle /// Raycast against front and back triangle
FRONT_AND_BACK FRONT_AND_BACK
}; };
// Class TriangleShape // Class TriangleShape
@ -53,85 +32,85 @@ enum TriangleRaycastSide {
*/ */
class TriangleShape : public ConvexShape { class TriangleShape : public ConvexShape {
protected: protected:
// -------------------- Attribute -------------------- // // -------------------- Attribute -------------------- //
/// Three points of the triangle /// Three points of the triangle
Vector3 mPoints[3]; Vector3 mPoints[3];
/// Raycast test type for the triangle (front, back, front-back) /// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide m_raycastTestType; TriangleRaycastSide m_raycastTestType;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
TriangleShape(const TriangleShape& shape); TriangleShape(const TriangleShape& shape);
/// Private assignment operator /// Private assignment operator
TriangleShape& operator=(const TriangleShape& shape); TriangleShape& operator=(const TriangleShape& shape);
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// 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;
/// Raycast method with feedback information /// 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) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
decimal margin = OBJECT_MARGIN); float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~TriangleShape(); virtual ~TriangleShape();
/// Return the local bounds of the shape in x, y and z directions. /// 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;
/// Set the local scaling vector of the collision shape /// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Update the AABB of a body using its collision shape /// Update the AABB of a body using its collision shape
virtual void computeAABB(AABB& aabb, const Transform& transform) const; virtual void computeAABB(AABB& aabb, const Transform& transform) const;
/// Return the raycast test type (front, back, front-back) /// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const; TriangleRaycastSide getRaycastTestType() const;
// Set the raycast test type (front, back, front-back) // Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType); void setRaycastTestType(TriangleRaycastSide testType);
/// Return the coordinates of a given vertex of the triangle /// Return the coordinates of a given vertex of the triangle
Vector3 getVertex(int index) const; Vector3 getVertex(int32_t index) const;
// ---------- Friendship ---------- // // ---------- Friendship ---------- //
friend class ConcaveMeshRaycastCallback; friend class ConcaveMeshRaycastCallback;
friend class TriangleOverlapCallback; friend class TriangleOverlapCallback;
}; };
// Return the number of bytes used by the collision shape // Return the number of bytes used by the collision shape
inline size_t TriangleShape::getSizeInBytes() const { inline size_t TriangleShape::getSizeInBytes() const {
return sizeof(TriangleShape); return sizeof(TriangleShape);
} }
// Return a local support point in a given direction without the object margin // Return a local support point in a given direction without the object margin
inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
return mPoints[dotProducts.getMaxAxis()]; return mPoints[dotProducts.getMaxAxis()];
} }
// Return the local bounds of the shape in x, y and z directions. // Return the local bounds of the shape in x, y and z directions.
@ -142,63 +121,63 @@ inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& d
*/ */
inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const {
const Vector3 xAxis(mPoints[0].x, mPoints[1].x, mPoints[2].x); const Vector3 xAxis(mPoints[0].x, mPoints[1].x, mPoints[2].x);
const Vector3 yAxis(mPoints[0].y, mPoints[1].y, mPoints[2].y); const Vector3 yAxis(mPoints[0].y, mPoints[1].y, mPoints[2].y);
const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z); const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z);
min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()); min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue());
max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()); max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue());
min -= Vector3(mMargin, mMargin, mMargin); min -= Vector3(mMargin, mMargin, mMargin);
max += Vector3(mMargin, mMargin, mMargin); max += Vector3(mMargin, mMargin, mMargin);
} }
// Set the local scaling vector of the collision shape // Set the local scaling vector of the collision shape
inline void TriangleShape::setLocalScaling(const Vector3& scaling) { inline void TriangleShape::setLocalScaling(const Vector3& scaling) {
mPoints[0] = (mPoints[0] / mScaling) * scaling; mPoints[0] = (mPoints[0] / mScaling) * scaling;
mPoints[1] = (mPoints[1] / mScaling) * scaling; mPoints[1] = (mPoints[1] / mScaling) * scaling;
mPoints[2] = (mPoints[2] / mScaling) * scaling; mPoints[2] = (mPoints[2] / mScaling) * scaling;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
// Return the local inertia tensor of the triangle shape // Return the local inertia tensor of the triangle shape
/** /**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
tensor.setToZero(); tensor.setToZero();
} }
// Update the AABB of a body using its collision shape // Update the AABB of a body using its collision shape
/** /**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates * computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape * @param transform Transform used to compute the AABB of the collision shape
*/ */
inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const { inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const {
const Vector3 worldPoint1 = transform * mPoints[0]; const Vector3 worldPoint1 = transform * mPoints[0];
const Vector3 worldPoint2 = transform * mPoints[1]; const Vector3 worldPoint2 = transform * mPoints[1];
const Vector3 worldPoint3 = transform * mPoints[2]; const Vector3 worldPoint3 = transform * mPoints[2];
const Vector3 xAxis(worldPoint1.x, worldPoint2.x, worldPoint3.x); const Vector3 xAxis(worldPoint1.x, worldPoint2.x, worldPoint3.x);
const Vector3 yAxis(worldPoint1.y, worldPoint2.y, worldPoint3.y); const Vector3 yAxis(worldPoint1.y, worldPoint2.y, worldPoint3.y);
const Vector3 zAxis(worldPoint1.z, worldPoint2.z, worldPoint3.z); const Vector3 zAxis(worldPoint1.z, worldPoint2.z, worldPoint3.z);
aabb.setMin(Vector3(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue())); aabb.setMin(Vector3(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()));
aabb.setMax(Vector3(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue())); aabb.setMax(Vector3(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()));
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return false; return false;
} }
// Return the raycast test type (front, back, front-back) // Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide TriangleShape::getRaycastTestType() const { inline TriangleRaycastSide TriangleShape::getRaycastTestType() const {
return m_raycastTestType; return m_raycastTestType;
} }
// Set the raycast test type (front, back, front-back) // Set the raycast test type (front, back, front-back)
@ -206,19 +185,17 @@ inline TriangleRaycastSide TriangleShape::getRaycastTestType() const {
* @param testType Raycast test type for the triangle (front, back, front-back) * @param testType Raycast test type for the triangle (front, back, front-back)
*/ */
inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
m_raycastTestType = testType; m_raycastTestType = testType;
} }
// Return the coordinates of a given vertex of the triangle // Return the coordinates of a given vertex of the triangle
/** /**
* @param index Index (0 to 2) of a vertex of the triangle * @param index Index (0 to 2) of a vertex of the triangle
*/ */
inline Vector3 TriangleShape::getVertex(int index) const { inline Vector3 TriangleShape::getVertex(int32_t index) const {
assert(index >= 0 && index < 3); assert(index >= 0 && index < 3);
return mPoints[index]; return mPoints[index];
} }
} }
#endif

237
ephysics/configuration.h
View File

@ -1,149 +1,108 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONFIGURATION_H
#define REACTPHYSICS3D_CONFIGURATION_H
// Libraries // Libraries
#include <limits> #include <limits>
#include <cfloat> #include <cfloat>
#include <utility> #include <utility>
#include <ephysics/decimal.h> #include <cstdint>
// Windows platform
#if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__)
#define WINDOWS_OS
#elif defined(__APPLE__) // Apple platform
#define APPLE_OS
#elif defined(__linux__) || defined(linux) || defined(__linux) // Linux platform
#define LINUX_OS
#endif
/// Namespace reactphysics3d /// Namespace reactphysics3d
namespace reactphysics3d { namespace reactphysics3d {
// ------------------- Type definitions ------------------- //
// ------------------- Type definitions ------------------- // typedef uint64_t bodyindex;
typedef std::pair<bodyindex, bodyindex> bodyindexpair;
typedef unsigned int uint;
typedef long unsigned int luint; // ------------------- Enumerations ------------------- //
typedef luint bodyindex;
typedef std::pair<bodyindex, bodyindex> bodyindexpair; /// Position correction technique used in the constraint solver (for joints).
/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations.
typedef signed short int16; /// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default.
typedef signed int int32; enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
typedef unsigned short uint16;
typedef unsigned int uint32; /// Position correction technique used in the contact solver (for contacts)
/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
// ------------------- Enumerations ------------------- // /// in some situations (due to error correction factor being added to
/// the bodies momentum).
/// Position correction technique used in the constraint solver (for joints). /// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations. /// bodies momentum. This is the option used by default.
/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default. enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
// ------------------- Constants ------------------- //
/// Position correction technique used in the contact solver (for contacts)
/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness /// Smallest float value (negative)
/// in some situations (due to error correction factor being added to const float DECIMAL_SMALLEST = - std::numeric_limits<float>::max();
/// the bodies momentum).
/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the /// Maximum float value
/// bodies momentum. This is the option used by default. const float DECIMAL_LARGEST = std::numeric_limits<float>::max();
enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
/// Machine epsilon
// ------------------- Constants ------------------- // const float MACHINE_EPSILON = std::numeric_limits<float>::epsilon();
/// Smallest decimal value (negative) /// Pi constant
const decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max(); const float PI = float(3.14159265);
/// Maximum decimal value /// 2*Pi constant
const decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max(); const float PI_TIMES_2 = float(6.28318530);
/// Machine epsilon /// Default friction coefficient for a rigid body
const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon(); const float DEFAULT_FRICTION_COEFFICIENT = float(0.3);
/// Pi constant /// Default bounciness factor for a rigid body
const decimal PI = decimal(3.14159265); const float DEFAULT_BOUNCINESS = float(0.5);
/// 2*Pi constant /// Default rolling resistance
const decimal PI_TIMES_2 = decimal(6.28318530); const float DEFAULT_ROLLING_RESISTANCE = float(0.0);
/// Default friction coefficient for a rigid body /// True if the spleeping technique is enabled
const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3); const bool SPLEEPING_ENABLED = true;
/// Default bounciness factor for a rigid body /// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
const decimal DEFAULT_BOUNCINESS = decimal(0.5); const float OBJECT_MARGIN = float(0.04);
/// Default rolling resistance /// Distance threshold for two contact points for a valid persistent contact (in meters)
const decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0); const float PERSISTENT_CONTACT_DIST_THRESHOLD = float(0.03);
/// True if the spleeping technique is enabled /// Velocity threshold for contact velocity restitution
const bool SPLEEPING_ENABLED = true; const float RESTITUTION_VELOCITY_THRESHOLD = float(1.0);
/// Object margin for collision detection in meters (for the GJK-EPA Algorithm) /// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
const decimal OBJECT_MARGIN = decimal(0.04); const uint32_t DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
/// Distance threshold for two contact points for a valid persistent contact (in meters) /// Number of iterations when solving the position constraints of the Sequential Impulse technique
const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); const uint32_t DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
/// Velocity threshold for contact velocity restitution /// Time (in seconds) that a body must stay still to be considered sleeping
const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique /// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10; /// might enter sleeping mode.
const float DEFAULT_SLEEP_LINEAR_VELOCITY = float(0.02);
/// Number of iterations when solving the position constraints of the Sequential Impulse technique
const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; /// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
/// might enter sleeping mode
/// Time (in seconds) that a body must stay still to be considered sleeping const float DEFAULT_SLEEP_ANGULAR_VELOCITY = float(3.0 * (PI / 180.0));
const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
/// A body with a linear velocity smaller than the sleep linear velocity (in m/s) /// inflated with a constant gap to allow the collision shape to move a little bit
/// might enter sleeping mode. /// without triggering a large modification of the tree which can be costly
const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02); const float DYNAMIC_TREE_AABB_GAP = float(0.1);
/// A body with angular velocity smaller than the sleep angular velocity (in rad/s) /// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
/// might enter sleeping mode /// also inflated in direction of the linear motion of the body by mutliplying the
const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0)); /// followin constant with the linear velocity and the elapsed time between two frames.
const float DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = float(1.7);
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
/// inflated with a constant gap to allow the collision shape to move a little bit /// Maximum number of contact manifolds in an overlapping pair that involves two
/// without triggering a large modification of the tree which can be costly /// convex collision shapes.
const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1); const int32_t NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are /// Maximum number of contact manifolds in an overlapping pair that involves at
/// also inflated in direction of the linear motion of the body by mutliplying the /// least one concave collision shape.
/// followin constant with the linear velocity and the elapsed time between two frames. const int32_t NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
/// Maximum number of contact manifolds in an overlapping pair that involves two
/// convex collision shapes.
const int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// Maximum number of contact manifolds in an overlapping pair that involves at
/// least one concave collision shape.
const int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
} }
#endif

305
ephysics/constraint/BallAndSocketJoint.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/constraint/BallAndSocketJoint.h> #include <ephysics/constraint/BallAndSocketJoint.h>
@ -30,15 +11,15 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Static variables definition // Static variables definition
const decimal BallAndSocketJoint::BETA = decimal(0.2); const float BallAndSocketJoint::BETA = float(0.2);
// Constructor // Constructor
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
: Joint(jointInfo), mImpulse(Vector3(0, 0, 0)) { : Joint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
} }
// Destructor // Destructor
@ -49,186 +30,186 @@ BallAndSocketJoint::~BallAndSocketJoint() {
// Initialize before solving the constraint // Initialize before solving the constraint
void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array // Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies center of mass and orientations // Get the bodies center of mass and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld; const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld; const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1; mR1World = orientationBody1 * mLocalAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2; mR2World = orientationBody2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) // Compute the matrix K=JM^-1J^t (3x3 matrix)
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1 // Compute the inverse mass matrix K^-1
mInverseMassMatrix.setToZero(); mInverseMassMatrix.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse(); mInverseMassMatrix = massMatrix.getInverse();
} }
// Compute the bias "b" of the constraint // Compute the bias "b" of the constraint
mBiasVector.setToZero(); mBiasVector.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
decimal biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World); mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World);
} }
// If warm-starting is not enabled // If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) { if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulse // Reset the accumulated impulse
mImpulse.setToZero(); mImpulse.setToZero();
} }
} }
// Warm start the constraint (apply the previous impulse at the beginning of the step) // Warm start the constraint (apply the previous impulse at the beginning of the step)
void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) { void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities // Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Compute the impulse P=J^T * lambda for the body 1 // Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -mImpulse; const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1; v1 += mBody1->mMassInverse * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2 // Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the body to the body 2 // Apply the impulse to the body to the body 2
v2 += mBody2->mMassInverse * mImpulse; v2 += mBody2->mMassInverse * mImpulse;
w2 += mI2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
// Solve the velocity constraint // Solve the velocity constraint
void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities // Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Compute J*v // Compute J*v
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
mImpulse += deltaLambda; mImpulse += deltaLambda;
// Compute the impulse P=J^T * lambda for the body 1 // Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -deltaLambda; const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1; v1 += mBody1->mMassInverse * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2 // Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += mBody2->mMassInverse * deltaLambda; v2 += mBody2->mMassInverse * deltaLambda;
w2 += mI2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
// Solve the position constraint (for position error correction) // Solve the position constraint (for position error correction)
void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do // If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method // do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies center of mass and orientations // Get the bodies center of mass and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1]; Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2]; Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1; mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2; mR2World = q2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2; float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrix.setToZero(); mInverseMassMatrix.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse(); mInverseMassMatrix = massMatrix.getInverse();
} }
// Compute the constraint error (value of the C(x) function) // Compute the constraint error (value of the C(x) function)
const Vector3 constraintError = (x2 + mR2World - x1 - mR1World); const Vector3 constraintError = (x2 + mR2World - x1 - mR1World);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
// TODO : Do not solve the system by computing the inverse each time and multiplying with the // TODO : Do not solve the system by computing the inverse each time and multiplying with the
// right-hand side vector but instead use a method to directly solve the linear system. // right-hand side vector but instead use a method to directly solve the linear system.
const Vector3 lambda = mInverseMassMatrix * (-constraintError); const Vector3 lambda = mInverseMassMatrix * (-constraintError);
// Compute the impulse of body 1 // Compute the impulse of body 1
const Vector3 linearImpulseBody1 = -lambda; const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(mR1World); const Vector3 angularImpulseBody1 = lambda.cross(mR1World);
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1; const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body center of mass and orientation of body 1 // Update the body center of mass and orientation of body 1
x1 += v1; x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
const Vector3 angularImpulseBody2 = -lambda.cross(mR2World); const Vector3 angularImpulseBody2 = -lambda.cross(mR2World);
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambda; const Vector3 v2 = inverseMassBody2 * lambda;
const Vector3 w2 = mI2 * angularImpulseBody2; const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
} }

155
ephysics/constraint/BallAndSocketJoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_BALL_AND_SOCKET_JOINT_H
#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H
// Libraries // Libraries
#include <ephysics/constraint/Joint.h> #include <ephysics/constraint/Joint.h>
@ -39,24 +18,24 @@ namespace reactphysics3d {
*/ */
struct BallAndSocketJointInfo : public JointInfo { struct BallAndSocketJointInfo : public JointInfo {
public : public :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates) /// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace; Vector3 anchorPointWorldSpace;
/// Constructor /// Constructor
/** /**
* @param rigidBody1 Pointer to the first body of the joint * @param rigidBody1 Pointer to the first body of the joint
* @param rigidBody2 Pointer to the second body of the joint * @param rigidBody2 Pointer to the second body of the joint
* @param initAnchorPointWorldSpace The anchor point in world-space * @param initAnchorPointWorldSpace The anchor point in world-space
* coordinates * coordinates
*/ */
BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace) const Vector3& initAnchorPointWorldSpace)
: JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), : JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace) {} anchorPointWorldSpace(initAnchorPointWorldSpace) {}
}; };
// Class BallAndSocketJoint // Class BallAndSocketJoint
@ -67,81 +46,79 @@ struct BallAndSocketJointInfo : public JointInfo {
*/ */
class BallAndSocketJoint : public Joint { class BallAndSocketJoint : public Joint {
private : private :
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction // Beta value for the bias factor of position correction
static const decimal BETA; static const float BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1) /// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1; Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2; Vector3 mLocalAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World; Vector3 mR1World;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World; Vector3 mR2World;
/// Inertia tensor of body 1 (in world-space coordinates) /// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1; Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates) /// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2; Matrix3x3 mI2;
/// Bias vector for the constraint /// Bias vector for the constraint
Vector3 mBiasVector; Vector3 mBiasVector;
/// Inverse mass matrix K=JM^-1J^-t of the constraint /// Inverse mass matrix K=JM^-1J^-t of the constraint
Matrix3x3 mInverseMassMatrix; Matrix3x3 mInverseMassMatrix;
/// Accumulated impulse /// Accumulated impulse
Vector3 mImpulse; Vector3 mImpulse;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
BallAndSocketJoint(const BallAndSocketJoint& constraint); BallAndSocketJoint(const BallAndSocketJoint& constraint);
/// Private assignment operator /// Private assignment operator
BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint); BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint);
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint /// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step) /// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData); virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction) /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo); BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo);
/// Destructor /// Destructor
virtual ~BallAndSocketJoint(); virtual ~BallAndSocketJoint();
}; };
// Return the number of bytes used by the joint // Return the number of bytes used by the joint
inline size_t BallAndSocketJoint::getSizeInBytes() const { inline size_t BallAndSocketJoint::getSizeInBytes() const {
return sizeof(BallAndSocketJoint); return sizeof(BallAndSocketJoint);
} }
} }
#endif

59
ephysics/constraint/ContactPoint.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/constraint/ContactPoint.h> #include <ephysics/constraint/ContactPoint.h>
@ -32,23 +13,23 @@ using namespace std;
// Constructor // Constructor
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
: mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()), : mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()),
mNormal(contactInfo.normal), mNormal(contactInfo.normal),
mPenetrationDepth(contactInfo.penetrationDepth), mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1), mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2), mLocalPointOnBody2(contactInfo.localPoint2),
mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() * mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() *
contactInfo.shape1->getLocalToBodyTransform() * contactInfo.shape1->getLocalToBodyTransform() *
contactInfo.localPoint1), contactInfo.localPoint1),
mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() * mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() *
contactInfo.shape2->getLocalToBodyTransform() * contactInfo.shape2->getLocalToBodyTransform() *
contactInfo.localPoint2), contactInfo.localPoint2),
mIsRestingContact(false) { mIsRestingContact(false) {
mFrictionVectors[0] = Vector3(0, 0, 0); mFrictionVectors[0] = Vector3(0, 0, 0);
mFrictionVectors[1] = Vector3(0, 0, 0); mFrictionVectors[1] = Vector3(0, 0, 0);
assert(mPenetrationDepth > 0.0); assert(mPenetrationDepth > 0.0);
} }

349
ephysics/constraint/ContactPoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_POINT_H
#define REACTPHYSICS3D_CONTACT_POINT_H
// Libraries // Libraries
#include <ephysics/body/CollisionBody.h> #include <ephysics/body/CollisionBody.h>
@ -45,49 +24,49 @@ namespace reactphysics3d {
*/ */
struct ContactPointInfo { struct ContactPointInfo {
private: private:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
public: public:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// First proxy shape of the contact /// First proxy shape of the contact
ProxyShape* shape1; ProxyShape* shape1;
/// Second proxy shape of the contact /// Second proxy shape of the contact
ProxyShape* shape2; ProxyShape* shape2;
/// First collision shape /// First collision shape
const CollisionShape* collisionShape1; const CollisionShape* collisionShape1;
/// Second collision shape /// Second collision shape
const CollisionShape* collisionShape2; const CollisionShape* collisionShape2;
/// Normalized normal vector of the collision contact in world space /// Normalized normal vector of the collision contact in world space
Vector3 normal; Vector3 normal;
/// Penetration depth of the contact /// Penetration depth of the contact
decimal penetrationDepth; float penetrationDepth;
/// Contact point of body 1 in local space of body 1 /// Contact point of body 1 in local space of body 1
Vector3 localPoint1; Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2 /// Contact point of body 2 in local space of body 2
Vector3 localPoint2; Vector3 localPoint2;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1, ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1,
const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth, const CollisionShape* collShape2, const Vector3& normal, float penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2) const Vector3& localPoint1, const Vector3& localPoint2)
: shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2), : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2),
normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
localPoint2(localPoint2) { localPoint2(localPoint2) {
} }
}; };
// Class ContactPoint // Class ContactPoint
@ -97,279 +76,277 @@ struct ContactPointInfo {
*/ */
class ContactPoint { class ContactPoint {
private : private :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// First rigid body of the contact /// First rigid body of the contact
CollisionBody* mBody1; CollisionBody* mBody1;
/// Second rigid body of the contact /// Second rigid body of the contact
CollisionBody* mBody2; CollisionBody* mBody2;
/// Normalized normal vector of the contact (from body1 toward body2) in world space /// Normalized normal vector of the contact (from body1 toward body2) in world space
const Vector3 mNormal; const Vector3 mNormal;
/// Penetration depth /// Penetration depth
decimal mPenetrationDepth; float mPenetrationDepth;
/// Contact point on body 1 in local space of body 1 /// Contact point on body 1 in local space of body 1
const Vector3 mLocalPointOnBody1; const Vector3 mLocalPointOnBody1;
/// Contact point on body 2 in local space of body 2 /// Contact point on body 2 in local space of body 2
const Vector3 mLocalPointOnBody2; const Vector3 mLocalPointOnBody2;
/// Contact point on body 1 in world space /// Contact point on body 1 in world space
Vector3 mWorldPointOnBody1; Vector3 mWorldPointOnBody1;
/// Contact point on body 2 in world space /// Contact point on body 2 in world space
Vector3 mWorldPointOnBody2; Vector3 mWorldPointOnBody2;
/// True if the contact is a resting contact (exists for more than one time step) /// True if the contact is a resting contact (exists for more than one time step)
bool mIsRestingContact; bool mIsRestingContact;
/// Two orthogonal vectors that span the tangential friction plane /// Two orthogonal vectors that span the tangential friction plane
Vector3 mFrictionVectors[2]; Vector3 mFrictionVectors[2];
/// Cached penetration impulse /// Cached penetration impulse
decimal mPenetrationImpulse; float mPenetrationImpulse;
/// Cached first friction impulse /// Cached first friction impulse
decimal mFrictionImpulse1; float mFrictionImpulse1;
/// Cached second friction impulse /// Cached second friction impulse
decimal mFrictionImpulse2; float mFrictionImpulse2;
/// Cached rolling resistance impulse /// Cached rolling resistance impulse
Vector3 mRollingResistanceImpulse; Vector3 mRollingResistanceImpulse;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ContactPoint(const ContactPoint& contact); ContactPoint(const ContactPoint& contact);
/// Private assignment operator /// Private assignment operator
ContactPoint& operator=(const ContactPoint& contact); ContactPoint& operator=(const ContactPoint& contact);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactPoint(const ContactPointInfo& contactInfo); ContactPoint(const ContactPointInfo& contactInfo);
/// Destructor /// Destructor
~ContactPoint(); ~ContactPoint();
/// Return the reference to the body 1 /// Return the reference to the body 1
CollisionBody* getBody1() const; CollisionBody* getBody1() const;
/// Return the reference to the body 2 /// Return the reference to the body 2
CollisionBody* getBody2() const; CollisionBody* getBody2() const;
/// Return the normal vector of the contact /// Return the normal vector of the contact
Vector3 getNormal() const; Vector3 getNormal() const;
/// Set the penetration depth of the contact /// Set the penetration depth of the contact
void setPenetrationDepth(decimal penetrationDepth); void setPenetrationDepth(float penetrationDepth);
/// Return the contact local point on body 1 /// Return the contact local point on body 1
Vector3 getLocalPointOnBody1() const; Vector3 getLocalPointOnBody1() const;
/// Return the contact local point on body 2 /// Return the contact local point on body 2
Vector3 getLocalPointOnBody2() const; Vector3 getLocalPointOnBody2() const;
/// Return the contact world point on body 1 /// Return the contact world point on body 1
Vector3 getWorldPointOnBody1() const; Vector3 getWorldPointOnBody1() const;
/// Return the contact world point on body 2 /// Return the contact world point on body 2
Vector3 getWorldPointOnBody2() const; Vector3 getWorldPointOnBody2() const;
/// Return the cached penetration impulse /// Return the cached penetration impulse
decimal getPenetrationImpulse() const; float getPenetrationImpulse() const;
/// Return the cached first friction impulse /// Return the cached first friction impulse
decimal getFrictionImpulse1() const; float getFrictionImpulse1() const;
/// Return the cached second friction impulse /// Return the cached second friction impulse
decimal getFrictionImpulse2() const; float getFrictionImpulse2() const;
/// Return the cached rolling resistance impulse /// Return the cached rolling resistance impulse
Vector3 getRollingResistanceImpulse() const; Vector3 getRollingResistanceImpulse() const;
/// Set the cached penetration impulse /// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse); void setPenetrationImpulse(float impulse);
/// Set the first cached friction impulse /// Set the first cached friction impulse
void setFrictionImpulse1(decimal impulse); void setFrictionImpulse1(float impulse);
/// Set the second cached friction impulse /// Set the second cached friction impulse
void setFrictionImpulse2(decimal impulse); void setFrictionImpulse2(float impulse);
/// Set the cached rolling resistance impulse /// Set the cached rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& impulse); void setRollingResistanceImpulse(const Vector3& impulse);
/// Set the contact world point on body 1 /// Set the contact world point on body 1
void setWorldPointOnBody1(const Vector3& worldPoint); void setWorldPointOnBody1(const Vector3& worldPoint);
/// Set the contact world point on body 2 /// Set the contact world point on body 2
void setWorldPointOnBody2(const Vector3& worldPoint); void setWorldPointOnBody2(const Vector3& worldPoint);
/// Return true if the contact is a resting contact /// Return true if the contact is a resting contact
bool getIsRestingContact() const; bool getIsRestingContact() const;
/// Set the mIsRestingContact variable /// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact); void setIsRestingContact(bool isRestingContact);
/// Get the first friction vector /// Get the first friction vector
Vector3 getFrictionVector1() const; Vector3 getFrictionVector1() const;
/// Set the first friction vector /// Set the first friction vector
void setFrictionVector1(const Vector3& frictionVector1); void setFrictionVector1(const Vector3& frictionVector1);
/// Get the second friction vector /// Get the second friction vector
Vector3 getFrictionVector2() const; Vector3 getFrictionVector2() const;
/// Set the second friction vector /// Set the second friction vector
void setFrictionVector2(const Vector3& frictionVector2); void setFrictionVector2(const Vector3& frictionVector2);
/// Return the penetration depth /// Return the penetration depth
decimal getPenetrationDepth() const; float getPenetrationDepth() const;
/// Return the number of bytes used by the contact point /// Return the number of bytes used by the contact point
size_t getSizeInBytes() const; size_t getSizeInBytes() const;
}; };
// Return the reference to the body 1 // Return the reference to the body 1
inline CollisionBody* ContactPoint::getBody1() const { inline CollisionBody* ContactPoint::getBody1() const {
return mBody1; return mBody1;
} }
// Return the reference to the body 2 // Return the reference to the body 2
inline CollisionBody* ContactPoint::getBody2() const { inline CollisionBody* ContactPoint::getBody2() const {
return mBody2; return mBody2;
} }
// Return the normal vector of the contact // Return the normal vector of the contact
inline Vector3 ContactPoint::getNormal() const { inline Vector3 ContactPoint::getNormal() const {
return mNormal; return mNormal;
} }
// Set the penetration depth of the contact // Set the penetration depth of the contact
inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) { inline void ContactPoint::setPenetrationDepth(float penetrationDepth) {
this->mPenetrationDepth = penetrationDepth; this->mPenetrationDepth = penetrationDepth;
} }
// Return the contact point on body 1 // Return the contact point on body 1
inline Vector3 ContactPoint::getLocalPointOnBody1() const { inline Vector3 ContactPoint::getLocalPointOnBody1() const {
return mLocalPointOnBody1; return mLocalPointOnBody1;
} }
// Return the contact point on body 2 // Return the contact point on body 2
inline Vector3 ContactPoint::getLocalPointOnBody2() const { inline Vector3 ContactPoint::getLocalPointOnBody2() const {
return mLocalPointOnBody2; return mLocalPointOnBody2;
} }
// Return the contact world point on body 1 // Return the contact world point on body 1
inline Vector3 ContactPoint::getWorldPointOnBody1() const { inline Vector3 ContactPoint::getWorldPointOnBody1() const {
return mWorldPointOnBody1; return mWorldPointOnBody1;
} }
// Return the contact world point on body 2 // Return the contact world point on body 2
inline Vector3 ContactPoint::getWorldPointOnBody2() const { inline Vector3 ContactPoint::getWorldPointOnBody2() const {
return mWorldPointOnBody2; return mWorldPointOnBody2;
} }
// Return the cached penetration impulse // Return the cached penetration impulse
inline decimal ContactPoint::getPenetrationImpulse() const { inline float ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse; return mPenetrationImpulse;
} }
// Return the cached first friction impulse // Return the cached first friction impulse
inline decimal ContactPoint::getFrictionImpulse1() const { inline float ContactPoint::getFrictionImpulse1() const {
return mFrictionImpulse1; return mFrictionImpulse1;
} }
// Return the cached second friction impulse // Return the cached second friction impulse
inline decimal ContactPoint::getFrictionImpulse2() const { inline float ContactPoint::getFrictionImpulse2() const {
return mFrictionImpulse2; return mFrictionImpulse2;
} }
// Return the cached rolling resistance impulse // Return the cached rolling resistance impulse
inline Vector3 ContactPoint::getRollingResistanceImpulse() const { inline Vector3 ContactPoint::getRollingResistanceImpulse() const {
return mRollingResistanceImpulse; return mRollingResistanceImpulse;
} }
// Set the cached penetration impulse // Set the cached penetration impulse
inline void ContactPoint::setPenetrationImpulse(decimal impulse) { inline void ContactPoint::setPenetrationImpulse(float impulse) {
mPenetrationImpulse = impulse; mPenetrationImpulse = impulse;
} }
// Set the first cached friction impulse // Set the first cached friction impulse
inline void ContactPoint::setFrictionImpulse1(decimal impulse) { inline void ContactPoint::setFrictionImpulse1(float impulse) {
mFrictionImpulse1 = impulse; mFrictionImpulse1 = impulse;
} }
// Set the second cached friction impulse // Set the second cached friction impulse
inline void ContactPoint::setFrictionImpulse2(decimal impulse) { inline void ContactPoint::setFrictionImpulse2(float impulse) {
mFrictionImpulse2 = impulse; mFrictionImpulse2 = impulse;
} }
// Set the cached rolling resistance impulse // Set the cached rolling resistance impulse
inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) { inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) {
mRollingResistanceImpulse = impulse; mRollingResistanceImpulse = impulse;
} }
// Set the contact world point on body 1 // Set the contact world point on body 1
inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
mWorldPointOnBody1 = worldPoint; mWorldPointOnBody1 = worldPoint;
} }
// Set the contact world point on body 2 // Set the contact world point on body 2
inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) { inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) {
mWorldPointOnBody2 = worldPoint; mWorldPointOnBody2 = worldPoint;
} }
// Return true if the contact is a resting contact // Return true if the contact is a resting contact
inline bool ContactPoint::getIsRestingContact() const { inline bool ContactPoint::getIsRestingContact() const {
return mIsRestingContact; return mIsRestingContact;
} }
// Set the mIsRestingContact variable // Set the mIsRestingContact variable
inline void ContactPoint::setIsRestingContact(bool isRestingContact) { inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
mIsRestingContact = isRestingContact; mIsRestingContact = isRestingContact;
} }
// Get the first friction vector // Get the first friction vector
inline Vector3 ContactPoint::getFrictionVector1() const { inline Vector3 ContactPoint::getFrictionVector1() const {
return mFrictionVectors[0]; return mFrictionVectors[0];
} }
// Set the first friction vector // Set the first friction vector
inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) { inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) {
mFrictionVectors[0] = frictionVector1; mFrictionVectors[0] = frictionVector1;
} }
// Get the second friction vector // Get the second friction vector
inline Vector3 ContactPoint::getFrictionVector2() const { inline Vector3 ContactPoint::getFrictionVector2() const {
return mFrictionVectors[1]; return mFrictionVectors[1];
} }
// Set the second friction vector // Set the second friction vector
inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) { inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) {
mFrictionVectors[1] = frictionVector2; mFrictionVectors[1] = frictionVector2;
} }
// Return the penetration depth of the contact // Return the penetration depth of the contact
inline decimal ContactPoint::getPenetrationDepth() const { inline float ContactPoint::getPenetrationDepth() const {
return mPenetrationDepth; return mPenetrationDepth;
} }
// Return the number of bytes used by the contact point // Return the number of bytes used by the contact point
inline size_t ContactPoint::getSizeInBytes() const { inline size_t ContactPoint::getSizeInBytes() const {
return sizeof(ContactPoint); return sizeof(ContactPoint);
} }
} }
#endif

447
ephysics/constraint/FixedJoint.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/constraint/FixedJoint.h> #include <ephysics/constraint/FixedJoint.h>
@ -30,23 +11,23 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Static variables definition // Static variables definition
const decimal FixedJoint::BETA = decimal(0.2); const float FixedJoint::BETA = float(0.2);
// Constructor // Constructor
FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
: Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { : Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) {
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
const Transform& transform1 = mBody1->getTransform(); const Transform& transform1 = mBody1->getTransform();
const Transform& transform2 = mBody2->getTransform(); const Transform& transform2 = mBody2->getTransform();
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies // Compute the inverse of the initial orientation difference between the two bodies
mInitOrientationDifferenceInv = transform2.getOrientation() * mInitOrientationDifferenceInv = transform2.getOrientation() *
transform1.getOrientation().getInverse(); transform1.getOrientation().getInverse();
mInitOrientationDifferenceInv.normalize(); mInitOrientationDifferenceInv.normalize();
mInitOrientationDifferenceInv.inverse(); mInitOrientationDifferenceInv.inverse();
} }
// Destructor // Destructor
@ -57,273 +38,273 @@ FixedJoint::~FixedJoint() {
// Initialize before solving the constraint // Initialize before solving the constraint
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array // Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations // Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld; const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld; const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1; mR1World = orientationBody1 * mLocalAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2; mR2World = orientationBody2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1 for the 3 translation constraints // Compute the inverse mass matrix K^-1 for the 3 translation constraints
mInverseMassMatrixTranslation.setToZero(); mInverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); mInverseMassMatrixTranslation = massMatrix.getInverse();
} }
// Compute the bias "b" of the constraint for the 3 translation constraints // Compute the bias "b" of the constraint for the 3 translation constraints
decimal biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
mBiasTranslation.setToZero(); mBiasTranslation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
} }
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotation = mI1 + mI2; mInverseMassMatrixRotation = mI1 + mI2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
} }
// Compute the bias "b" for the 3 rotation constraints // Compute the bias "b" for the 3 rotation constraints
mBiasRotation.setToZero(); mBiasRotation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV(); mBiasRotation = biasFactor * float(2.0) * qError.getVectorV();
} }
// If warm-starting is not enabled // If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) { if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulses // Reset the accumulated impulses
mImpulseTranslation.setToZero(); mImpulseTranslation.setToZero();
mImpulseRotation.setToZero(); mImpulseRotation.setToZero();
} }
} }
// Warm start the constraint (apply the previous impulse at the beginning of the step) // Warm start the constraint (apply the previous impulse at the beginning of the step)
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities // Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies // Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse; const float inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse; const float inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation; Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 += -mImpulseRotation; angularImpulseBody1 += -mImpulseRotation;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2 // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
angularImpulseBody2 += mImpulseRotation; angularImpulseBody2 += mImpulseRotation;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * mImpulseTranslation; v2 += inverseMassBody2 * mImpulseTranslation;
w2 += mI2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
// Solve the velocity constraint // Solve the velocity constraint
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities // Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies // Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute J*v for the 3 translation constraints // Compute J*v for the 3 translation constraints
const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = mInverseMassMatrixTranslation * const Vector3 deltaLambda = mInverseMassMatrixTranslation *
(-JvTranslation - mBiasTranslation); (-JvTranslation - mBiasTranslation);
mImpulseTranslation += deltaLambda; mImpulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda for body 1 // Compute the impulse P=J^T * lambda for body 1
const Vector3 linearImpulseBody1 = -deltaLambda; const Vector3 linearImpulseBody1 = -deltaLambda;
Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for body 2 // Compute the impulse P=J^T * lambda for body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambda; v2 += inverseMassBody2 * deltaLambda;
w2 += mI2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation constraints // Compute J*v for the 3 rotation constraints
const Vector3 JvRotation = w2 - w1; const Vector3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
mImpulseRotation += deltaLambda2; mImpulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 = -deltaLambda2; angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 += mI2 * deltaLambda2; w2 += mI2 * deltaLambda2;
} }
// Solve the position constraint (for position error correction) // Solve the position constraint (for position error correction)
void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do // If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method // do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations // Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1]; Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2]; Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1; mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2; mR2World = q2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero(); mInverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); mInverseMassMatrixTranslation = massMatrix.getInverse();
} }
// Compute position error for the 3 translation constraints // Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World;
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1 // Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation; Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * angularImpulseBody1; Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation; const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = mI2 * angularImpulseBody2; Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotation = mI1 + mI2; mInverseMassMatrixRotation = mI1 + mI2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
} }
// Compute the position error for the 3 rotation constraints // Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse(); Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); const Vector3 errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation; angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
w1 = mI1 * angularImpulseBody1; w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
w2 = mI2 * lambdaRotation; w2 = mI2 * lambdaRotation;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
} }

171
ephysics/constraint/FixedJoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_FIXED_JOINT_H
#define REACTPHYSICS3D_FIXED_JOINT_H
// Libraries // Libraries
#include <ephysics/constraint/Joint.h> #include <ephysics/constraint/Joint.h>
@ -39,24 +18,24 @@ namespace reactphysics3d {
*/ */
struct FixedJointInfo : public JointInfo { struct FixedJointInfo : public JointInfo {
public : public :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates) /// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace; Vector3 anchorPointWorldSpace;
/// Constructor /// Constructor
/** /**
* @param rigidBody1 The first body of the joint * @param rigidBody1 The first body of the joint
* @param rigidBody2 The second body of the joint * @param rigidBody2 The second body of the joint
* @param initAnchorPointWorldSpace The initial anchor point of the joint in * @param initAnchorPointWorldSpace The initial anchor point of the joint in
* world-space coordinates * world-space coordinates
*/ */
FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace) const Vector3& initAnchorPointWorldSpace)
: JointInfo(rigidBody1, rigidBody2, FIXEDJOINT), : JointInfo(rigidBody1, rigidBody2, FIXEDJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace){} anchorPointWorldSpace(initAnchorPointWorldSpace){}
}; };
// Class FixedJoint // Class FixedJoint
@ -66,93 +45,91 @@ struct FixedJointInfo : public JointInfo {
*/ */
class FixedJoint : public Joint { class FixedJoint : public Joint {
private : private :
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction // Beta value for the bias factor of position correction
static const decimal BETA; static const float BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1) /// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1; Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2; Vector3 mLocalAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World; Vector3 mR1World;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World; Vector3 mR2World;
/// Inertia tensor of body 1 (in world-space coordinates) /// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1; Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates) /// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2; Matrix3x3 mI2;
/// Accumulated impulse for the 3 translation constraints /// Accumulated impulse for the 3 translation constraints
Vector3 mImpulseTranslation; Vector3 mImpulseTranslation;
/// Accumulate impulse for the 3 rotation constraints /// Accumulate impulse for the 3 rotation constraints
Vector3 mImpulseRotation; Vector3 mImpulseRotation;
/// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix)
Matrix3x3 mInverseMassMatrixTranslation; Matrix3x3 mInverseMassMatrixTranslation;
/// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix)
Matrix3x3 mInverseMassMatrixRotation; Matrix3x3 mInverseMassMatrixRotation;
/// Bias vector for the 3 translation constraints /// Bias vector for the 3 translation constraints
Vector3 mBiasTranslation; Vector3 mBiasTranslation;
/// Bias vector for the 3 rotation constraints /// Bias vector for the 3 rotation constraints
Vector3 mBiasRotation; Vector3 mBiasRotation;
/// Inverse of the initial orientation difference between the two bodies /// Inverse of the initial orientation difference between the two bodies
Quaternion mInitOrientationDifferenceInv; Quaternion mInitOrientationDifferenceInv;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
FixedJoint(const FixedJoint& constraint); FixedJoint(const FixedJoint& constraint);
/// Private assignment operator /// Private assignment operator
FixedJoint& operator=(const FixedJoint& constraint); FixedJoint& operator=(const FixedJoint& constraint);
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint /// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step) /// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData); virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction) /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
FixedJoint(const FixedJointInfo& jointInfo); FixedJoint(const FixedJointInfo& jointInfo);
/// Destructor /// Destructor
virtual ~FixedJoint(); virtual ~FixedJoint();
}; };
// Return the number of bytes used by the joint // Return the number of bytes used by the joint
inline size_t FixedJoint::getSizeInBytes() const { inline size_t FixedJoint::getSizeInBytes() const {
return sizeof(FixedJoint); return sizeof(FixedJoint);
} }
} }
#endif

1101
ephysics/constraint/HingeJoint.cpp
View File

File diff suppressed because it is too large Load Diff

485
ephysics/constraint/HingeJoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_HINGE_JOINT_H
#define REACTPHYSICS3D_HINGE_JOINT_H
// Libraries // Libraries
#include <ephysics/constraint/Joint.h> #include <ephysics/constraint/Joint.h>
@ -39,97 +18,97 @@ namespace reactphysics3d {
*/ */
struct HingeJointInfo : public JointInfo { struct HingeJointInfo : public JointInfo {
public : public :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates) /// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace; Vector3 anchorPointWorldSpace;
/// Hinge rotation axis (in world-space coordinates) /// Hinge rotation axis (in world-space coordinates)
Vector3 rotationAxisWorld; Vector3 rotationAxisWorld;
/// True if the hinge joint limits are enabled /// True if the hinge joint limits are enabled
bool isLimitEnabled; bool isLimitEnabled;
/// True if the hinge joint motor is enabled /// True if the hinge joint motor is enabled
bool isMotorEnabled; bool isMotorEnabled;
/// Minimum allowed rotation angle (in radian) if limits are enabled. /// Minimum allowed rotation angle (in radian) if limits are enabled.
/// The angle must be in the range [-2*pi, 0] /// The angle must be in the range [-2*pi, 0]
decimal minAngleLimit; float minAngleLimit;
/// Maximum allowed rotation angle (in radian) if limits are enabled. /// Maximum allowed rotation angle (in radian) if limits are enabled.
/// The angle must be in the range [0, 2*pi] /// The angle must be in the range [0, 2*pi]
decimal maxAngleLimit; float maxAngleLimit;
/// Motor speed (in radian/second) /// Motor speed (in radian/second)
decimal motorSpeed; float motorSpeed;
/// Maximum motor torque (in Newtons * meters) that can be applied to reach /// Maximum motor torque (in Newtons * meters) that can be applied to reach
/// to desired motor speed /// to desired motor speed
decimal maxMotorTorque; float maxMotorTorque;
/// Constructor without limits and without motor /// Constructor without limits and without motor
/** /**
* @param rigidBody1 The first body of the joint * @param rigidBody1 The first body of the joint
* @param rigidBody2 The second body of the joint * @param rigidBody2 The second body of the joint
* @param initAnchorPointWorldSpace The initial anchor point in world-space * @param initAnchorPointWorldSpace The initial anchor point in world-space
* coordinates * coordinates
* @param initRotationAxisWorld The initial rotation axis in world-space * @param initRotationAxisWorld The initial rotation axis in world-space
* coordinates * coordinates
*/ */
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld) const Vector3& initRotationAxisWorld)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), : JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false),
isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1),
motorSpeed(0), maxMotorTorque(0) {} motorSpeed(0), maxMotorTorque(0) {}
/// Constructor with limits but without motor /// Constructor with limits but without motor
/** /**
* @param rigidBody1 The first body of the joint * @param rigidBody1 The first body of the joint
* @param rigidBody2 The second body of the joint * @param rigidBody2 The second body of the joint
* @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates
* @param initRotationAxisWorld The intial rotation axis in world-space coordinates * @param initRotationAxisWorld The int32_tial rotation axis in world-space coordinates
* @param initMinAngleLimit The initial minimum limit angle (in radian) * @param initMinAngleLimit The initial minimum limit angle (in radian)
* @param initMaxAngleLimit The initial maximum limit angle (in radian) * @param initMaxAngleLimit The initial maximum limit angle (in radian)
*/ */
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld, const Vector3& initRotationAxisWorld,
decimal initMinAngleLimit, decimal initMaxAngleLimit) float initMinAngleLimit, float initMaxAngleLimit)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), : JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit), isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(0), maxAngleLimit(initMaxAngleLimit), motorSpeed(0),
maxMotorTorque(0) {} maxMotorTorque(0) {}
/// Constructor with limits and motor /// Constructor with limits and motor
/** /**
* @param rigidBody1 The first body of the joint * @param rigidBody1 The first body of the joint
* @param rigidBody2 The second body of the joint * @param rigidBody2 The second body of the joint
* @param initAnchorPointWorldSpace The initial anchor point in world-space * @param initAnchorPointWorldSpace The initial anchor point in world-space
* @param initRotationAxisWorld The initial rotation axis in world-space * @param initRotationAxisWorld The initial rotation axis in world-space
* @param initMinAngleLimit The initial minimum limit angle (in radian) * @param initMinAngleLimit The initial minimum limit angle (in radian)
* @param initMaxAngleLimit The initial maximum limit angle (in radian) * @param initMaxAngleLimit The initial maximum limit angle (in radian)
* @param initMotorSpeed The initial motor speed of the joint (in radian per second) * @param initMotorSpeed The initial motor speed of the joint (in radian per second)
* @param initMaxMotorTorque The initial maximum motor torque (in Newtons) * @param initMaxMotorTorque The initial maximum motor torque (in Newtons)
*/ */
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld, const Vector3& initRotationAxisWorld,
decimal initMinAngleLimit, decimal initMaxAngleLimit, float initMinAngleLimit, float initMaxAngleLimit,
decimal initMotorSpeed, decimal initMaxMotorTorque) float initMotorSpeed, float initMaxMotorTorque)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), : JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit), isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed),
maxMotorTorque(initMaxMotorTorque) {} maxMotorTorque(initMaxMotorTorque) {}
}; };
// Class HingeJoint // Class HingeJoint
@ -140,202 +119,202 @@ struct HingeJointInfo : public JointInfo {
*/ */
class HingeJoint : public Joint { class HingeJoint : public Joint {
private : private :
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction // Beta value for the bias factor of position correction
static const decimal BETA; static const float BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1) /// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1; Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2; Vector3 mLocalAnchorPointBody2;
/// Hinge rotation axis (in local-space coordinates of body 1) /// Hinge rotation axis (in local-space coordinates of body 1)
Vector3 mHingeLocalAxisBody1; Vector3 mHingeLocalAxisBody1;
/// Hinge rotation axis (in local-space coordiantes of body 2) /// Hinge rotation axis (in local-space coordiantes of body 2)
Vector3 mHingeLocalAxisBody2; Vector3 mHingeLocalAxisBody2;
/// Inertia tensor of body 1 (in world-space coordinates) /// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1; Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates) /// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2; Matrix3x3 mI2;
/// Hinge rotation axis (in world-space coordinates) computed from body 1 /// Hinge rotation axis (in world-space coordinates) computed from body 1
Vector3 mA1; Vector3 mA1;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World; Vector3 mR1World;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World; Vector3 mR2World;
/// Cross product of vector b2 and a1 /// Cross product of vector b2 and a1
Vector3 mB2CrossA1; Vector3 mB2CrossA1;
/// Cross product of vector c2 and a1; /// Cross product of vector c2 and a1;
Vector3 mC2CrossA1; Vector3 mC2CrossA1;
/// Impulse for the 3 translation constraints /// Impulse for the 3 translation constraints
Vector3 mImpulseTranslation; Vector3 mImpulseTranslation;
/// Impulse for the 2 rotation constraints /// Impulse for the 2 rotation constraints
Vector2 mImpulseRotation; Vector2 mImpulseRotation;
/// Accumulated impulse for the lower limit constraint /// Accumulated impulse for the lower limit constraint
decimal mImpulseLowerLimit; float mImpulseLowerLimit;
/// Accumulated impulse for the upper limit constraint /// Accumulated impulse for the upper limit constraint
decimal mImpulseUpperLimit; float mImpulseUpperLimit;
/// Accumulated impulse for the motor constraint; /// Accumulated impulse for the motor constraint;
decimal mImpulseMotor; float mImpulseMotor;
/// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints
Matrix3x3 mInverseMassMatrixTranslation; Matrix3x3 mInverseMassMatrixTranslation;
/// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints
Matrix2x2 mInverseMassMatrixRotation; Matrix2x2 mInverseMassMatrixRotation;
/// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
decimal mInverseMassMatrixLimitMotor; float mInverseMassMatrixLimitMotor;
/// Inverse of mass matrix K=JM^-1J^t for the motor /// Inverse of mass matrix K=JM^-1J^t for the motor
decimal mInverseMassMatrixMotor; float mInverseMassMatrixMotor;
/// Bias vector for the error correction for the translation constraints /// Bias vector for the error correction for the translation constraints
Vector3 mBTranslation; Vector3 mBTranslation;
/// Bias vector for the error correction for the rotation constraints /// Bias vector for the error correction for the rotation constraints
Vector2 mBRotation; Vector2 mBRotation;
/// Bias of the lower limit constraint /// Bias of the lower limit constraint
decimal mBLowerLimit; float mBLowerLimit;
/// Bias of the upper limit constraint /// Bias of the upper limit constraint
decimal mBUpperLimit; float mBUpperLimit;
/// Inverse of the initial orientation difference between the bodies /// Inverse of the initial orientation difference between the bodies
Quaternion mInitOrientationDifferenceInv; Quaternion mInitOrientationDifferenceInv;
/// True if the joint limits are enabled /// True if the joint limits are enabled
bool mIsLimitEnabled; bool mIsLimitEnabled;
/// True if the motor of the joint in enabled /// True if the motor of the joint in enabled
bool mIsMotorEnabled; bool mIsMotorEnabled;
/// Lower limit (minimum allowed rotation angle in radian) /// Lower limit (minimum allowed rotation angle in radian)
decimal mLowerLimit; float mLowerLimit;
/// Upper limit (maximum translation distance) /// Upper limit (maximum translation distance)
decimal mUpperLimit; float mUpperLimit;
/// True if the lower limit is violated /// True if the lower limit is violated
bool mIsLowerLimitViolated; bool mIsLowerLimitViolated;
/// True if the upper limit is violated /// True if the upper limit is violated
bool mIsUpperLimitViolated; bool mIsUpperLimitViolated;
/// Motor speed (in rad/s) /// Motor speed (in rad/s)
decimal mMotorSpeed; float mMotorSpeed;
/// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorTorque; float mMaxMotorTorque;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
HingeJoint(const HingeJoint& constraint); HingeJoint(const HingeJoint& constraint);
/// Private assignment operator /// Private assignment operator
HingeJoint& operator=(const HingeJoint& constraint); HingeJoint& operator=(const HingeJoint& constraint);
/// Reset the limits /// Reset the limits
void resetLimits(); void resetLimits();
/// Given an angle in radian, this method returns the corresponding /// Given an angle in radian, this method returns the corresponding
/// angle in the range [-pi; pi] /// angle in the range [-pi; pi]
decimal computeNormalizedAngle(decimal angle) const; float computeNormalizedAngle(float angle) const;
/// Given an "inputAngle" in the range [-pi, pi], this method returns an /// Given an "inputAngle" in the range [-pi, pi], this method returns an
/// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
/// two angle limits in arguments. /// two angle limits in arguments.
decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, float computeCorrespondingAngleNearLimits(float inputAngle, float lowerLimitAngle,
decimal upperLimitAngle) const; float upperLimitAngle) const;
/// Compute the current angle around the hinge axis /// Compute the current angle around the hinge axis
decimal computeCurrentHingeAngle(const Quaternion& orientationBody1, float computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2); const Quaternion& orientationBody2);
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint /// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step) /// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData); virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction) /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
HingeJoint(const HingeJointInfo& jointInfo); HingeJoint(const HingeJointInfo& jointInfo);
/// Destructor /// Destructor
virtual ~HingeJoint(); virtual ~HingeJoint();
/// Return true if the limits or the joint are enabled /// Return true if the limits or the joint are enabled
bool isLimitEnabled() const; bool isLimitEnabled() const;
/// Return true if the motor of the joint is enabled /// Return true if the motor of the joint is enabled
bool isMotorEnabled() const; bool isMotorEnabled() const;
/// Enable/Disable the limits of the joint /// Enable/Disable the limits of the joint
void enableLimit(bool isLimitEnabled); void enableLimit(bool isLimitEnabled);
/// Enable/Disable the motor of the joint /// Enable/Disable the motor of the joint
void enableMotor(bool isMotorEnabled); void enableMotor(bool isMotorEnabled);
/// Return the minimum angle limit /// Return the minimum angle limit
decimal getMinAngleLimit() const; float getMinAngleLimit() const;
/// Set the minimum angle limit /// Set the minimum angle limit
void setMinAngleLimit(decimal lowerLimit); void setMinAngleLimit(float lowerLimit);
/// Return the maximum angle limit /// Return the maximum angle limit
decimal getMaxAngleLimit() const; float getMaxAngleLimit() const;
/// Set the maximum angle limit /// Set the maximum angle limit
void setMaxAngleLimit(decimal upperLimit); void setMaxAngleLimit(float upperLimit);
/// Return the motor speed /// Return the motor speed
decimal getMotorSpeed() const; float getMotorSpeed() const;
/// Set the motor speed /// Set the motor speed
void setMotorSpeed(decimal motorSpeed); void setMotorSpeed(float motorSpeed);
/// Return the maximum motor torque /// Return the maximum motor torque
decimal getMaxMotorTorque() const; float getMaxMotorTorque() const;
/// Set the maximum motor torque /// Set the maximum motor torque
void setMaxMotorTorque(decimal maxMotorTorque); void setMaxMotorTorque(float maxMotorTorque);
/// Return the intensity of the current torque applied for the joint motor /// Return the int32_tensity of the current torque applied for the joint motor
decimal getMotorTorque(decimal timeStep) const; float getMotorTorque(float timeStep) const;
}; };
// Return true if the limits of the joint are enabled // Return true if the limits of the joint are enabled
@ -343,7 +322,7 @@ class HingeJoint : public Joint {
* @return True if the limits of the joint are enabled and false otherwise * @return True if the limits of the joint are enabled and false otherwise
*/ */
inline bool HingeJoint::isLimitEnabled() const { inline bool HingeJoint::isLimitEnabled() const {
return mIsLimitEnabled; return mIsLimitEnabled;
} }
// Return true if the motor of the joint is enabled // Return true if the motor of the joint is enabled
@ -351,56 +330,54 @@ inline bool HingeJoint::isLimitEnabled() const {
* @return True if the motor of joint is enabled and false otherwise * @return True if the motor of joint is enabled and false otherwise
*/ */
inline bool HingeJoint::isMotorEnabled() const { inline bool HingeJoint::isMotorEnabled() const {
return mIsMotorEnabled; return mIsMotorEnabled;
} }
// Return the minimum angle limit // Return the minimum angle limit
/** /**
* @return The minimum limit angle of the joint (in radian) * @return The minimum limit angle of the joint (in radian)
*/ */
inline decimal HingeJoint::getMinAngleLimit() const { inline float HingeJoint::getMinAngleLimit() const {
return mLowerLimit; return mLowerLimit;
} }
// Return the maximum angle limit // Return the maximum angle limit
/** /**
* @return The maximum limit angle of the joint (in radian) * @return The maximum limit angle of the joint (in radian)
*/ */
inline decimal HingeJoint::getMaxAngleLimit() const { inline float HingeJoint::getMaxAngleLimit() const {
return mUpperLimit; return mUpperLimit;
} }
// Return the motor speed // Return the motor speed
/** /**
* @return The current speed of the joint motor (in radian per second) * @return The current speed of the joint motor (in radian per second)
*/ */
inline decimal HingeJoint::getMotorSpeed() const { inline float HingeJoint::getMotorSpeed() const {
return mMotorSpeed; return mMotorSpeed;
} }
// Return the maximum motor torque // Return the maximum motor torque
/** /**
* @return The maximum torque of the joint motor (in Newtons) * @return The maximum torque of the joint motor (in Newtons)
*/ */
inline decimal HingeJoint::getMaxMotorTorque() const { inline float HingeJoint::getMaxMotorTorque() const {
return mMaxMotorTorque; return mMaxMotorTorque;
} }
// Return the intensity of the current torque applied for the joint motor // Return the int32_tensity of the current torque applied for the joint motor
/** /**
* @param timeStep The current time step (in seconds) * @param timeStep The current time step (in seconds)
* @return The intensity of the current torque (in Newtons) of the joint motor * @return The int32_tensity of the current torque (in Newtons) of the joint motor
*/ */
inline decimal HingeJoint::getMotorTorque(decimal timeStep) const { inline float HingeJoint::getMotorTorque(float timeStep) const {
return mImpulseMotor / timeStep; return mImpulseMotor / timeStep;
} }
// Return the number of bytes used by the joint // Return the number of bytes used by the joint
inline size_t HingeJoint::getSizeInBytes() const { inline size_t HingeJoint::getSizeInBytes() const {
return sizeof(HingeJoint); return sizeof(HingeJoint);
} }
} }
#endif

40
ephysics/constraint/Joint.cpp
View File

@ -1,27 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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 // Libraries
#include <ephysics/constraint/Joint.h> #include <ephysics/constraint/Joint.h>
@ -30,12 +12,12 @@ using namespace reactphysics3d;
// Constructor // Constructor
Joint::Joint(const JointInfo& jointInfo) Joint::Joint(const JointInfo& jointInfo)
:mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type), :mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type),
mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique), mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique),
mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) {
assert(mBody1 != NULL); assert(mBody1 != NULL);
assert(mBody2 != NULL); assert(mBody2 != NULL);
} }
// Destructor // Destructor

235
ephysics/constraint/Joint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONSTRAINT_H
#define REACTPHYSICS3D_CONSTRAINT_H
// Libraries // Libraries
#include <ephysics/configuration.h> #include <ephysics/configuration.h>
@ -47,23 +26,23 @@ class Joint;
*/ */
struct JointListElement { struct JointListElement {
public: public:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the actual joint /// Pointer to the actual joint
Joint* joint; Joint* joint;
/// Next element of the list /// Next element of the list
JointListElement* next; JointListElement* next;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
JointListElement(Joint* initJoint, JointListElement* initNext) JointListElement(Joint* initJoint, JointListElement* initNext)
:joint(initJoint), next(initNext){ :joint(initJoint), next(initNext){
} }
}; };
// Structure JointInfo // Structure JointInfo
@ -72,41 +51,41 @@ struct JointListElement {
*/ */
struct JointInfo { struct JointInfo {
public : public :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// First rigid body of the joint /// First rigid body of the joint
RigidBody* body1; RigidBody* body1;
/// Second rigid body of the joint /// Second rigid body of the joint
RigidBody* body2; RigidBody* body2;
/// Type of the joint /// Type of the joint
JointType type; JointType type;
/// Position correction technique used for the constraint (used for joints). /// Position correction technique used for the constraint (used for joints).
/// By default, the BAUMGARTE technique is used /// By default, the BAUMGARTE technique is used
JointsPositionCorrectionTechnique positionCorrectionTechnique; JointsPositionCorrectionTechnique positionCorrectionTechnique;
/// True if the two bodies of the joint are allowed to collide with each other /// True if the two bodies of the joint are allowed to collide with each other
bool isCollisionEnabled; bool isCollisionEnabled;
/// Constructor /// Constructor
JointInfo(JointType constraintType) JointInfo(JointType constraintType)
: body1(NULL), body2(NULL), type(constraintType), : body1(NULL), body2(NULL), type(constraintType),
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
isCollisionEnabled(true) {} isCollisionEnabled(true) {}
/// Constructor /// Constructor
JointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, JointType constraintType) JointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, JointType constraintType)
: body1(rigidBody1), body2(rigidBody2), type(constraintType), : body1(rigidBody1), body2(rigidBody2), type(constraintType),
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
isCollisionEnabled(true) { isCollisionEnabled(true) {
} }
/// Destructor /// Destructor
virtual ~JointInfo() {} virtual ~JointInfo() {}
}; };
@ -116,90 +95,90 @@ struct JointInfo {
*/ */
class Joint { class Joint {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the first body of the joint /// Pointer to the first body of the joint
RigidBody* const mBody1; RigidBody* const mBody1;
/// Pointer to the second body of the joint /// Pointer to the second body of the joint
RigidBody* const mBody2; RigidBody* const mBody2;
/// Type of the joint /// Type of the joint
const JointType mType; const JointType mType;
/// Body 1 index in the velocity array to solve the constraint /// Body 1 index in the velocity array to solve the constraint
uint mIndexBody1; uint32_t mIndexBody1;
/// Body 2 index in the velocity array to solve the constraint /// Body 2 index in the velocity array to solve the constraint
uint mIndexBody2; uint32_t mIndexBody2;
/// Position correction technique used for the constraint (used for joints) /// Position correction technique used for the constraint (used for joints)
JointsPositionCorrectionTechnique mPositionCorrectionTechnique; JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
/// True if the two bodies of the constraint are allowed to collide with each other /// True if the two bodies of the constraint are allowed to collide with each other
bool mIsCollisionEnabled; bool mIsCollisionEnabled;
/// True if the joint has already been added into an island /// True if the joint has already been added int32_to an island
bool mIsAlreadyInIsland; bool mIsAlreadyInIsland;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
Joint(const Joint& constraint); Joint(const Joint& constraint);
/// Private assignment operator /// Private assignment operator
Joint& operator=(const Joint& constraint); Joint& operator=(const Joint& constraint);
/// Return true if the joint has already been added into an island /// Return true if the joint has already been added int32_to an island
bool isAlreadyInIsland() const; bool isAlreadyInIsland() const;
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const = 0; virtual size_t getSizeInBytes() const = 0;
/// Initialize before solving the joint /// Initialize before solving the joint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0;
/// Warm start the joint (apply the previous impulse at the beginning of the step) /// Warm start the joint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0; virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the position constraint /// Solve the position constraint
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Joint(const JointInfo& jointInfo); Joint(const JointInfo& jointInfo);
/// Destructor /// Destructor
virtual ~Joint(); virtual ~Joint();
/// Return the reference to the body 1 /// Return the reference to the body 1
RigidBody* getBody1() const; RigidBody* getBody1() const;
/// Return the reference to the body 2 /// Return the reference to the body 2
RigidBody* getBody2() const; RigidBody* getBody2() const;
/// Return true if the constraint is active /// Return true if the constraint is active
bool isActive() const; bool isActive() const;
/// Return the type of the constraint /// Return the type of the constraint
JointType getType() const; JointType getType() const;
/// Return true if the collision between the two bodies of the joint is enabled /// Return true if the collision between the two bodies of the joint is enabled
bool isCollisionEnabled() const; bool isCollisionEnabled() const;
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;
friend class Island; friend class Island;
friend class ConstraintSolver; friend class ConstraintSolver;
}; };
// Return the reference to the body 1 // Return the reference to the body 1
@ -207,7 +186,7 @@ class Joint {
* @return The first body involved in the joint * @return The first body involved in the joint
*/ */
inline RigidBody* Joint::getBody1() const { inline RigidBody* Joint::getBody1() const {
return mBody1; return mBody1;
} }
// Return the reference to the body 2 // Return the reference to the body 2
@ -215,7 +194,7 @@ inline RigidBody* Joint::getBody1() const {
* @return The second body involved in the joint * @return The second body involved in the joint
*/ */
inline RigidBody* Joint::getBody2() const { inline RigidBody* Joint::getBody2() const {
return mBody2; return mBody2;
} }
// Return true if the joint is active // Return true if the joint is active
@ -223,7 +202,7 @@ inline RigidBody* Joint::getBody2() const {
* @return True if the joint is active * @return True if the joint is active
*/ */
inline bool Joint::isActive() const { inline bool Joint::isActive() const {
return (mBody1->isActive() && mBody2->isActive()); return (mBody1->isActive() && mBody2->isActive());
} }
// Return the type of the joint // Return the type of the joint
@ -231,23 +210,21 @@ inline bool Joint::isActive() const {
* @return The type of the joint * @return The type of the joint
*/ */
inline JointType Joint::getType() const { inline JointType Joint::getType() const {
return mType; return mType;
} }
// Return true if the collision between the two bodies of the joint is enabled // Return true if the collision between the two bodies of the joint is enabled
/** /**
* @return True if the collision is enabled between the two bodies of the joint * @return True if the collision is enabled between the two bodies of the joint
* is enabled and false otherwise * is enabled and false otherwise
*/ */
inline bool Joint::isCollisionEnabled() const { inline bool Joint::isCollisionEnabled() const {
return mIsCollisionEnabled; return mIsCollisionEnabled;
} }
// Return true if the joint has already been added into an island // Return true if the joint has already been added int32_to an island
inline bool Joint::isAlreadyInIsland() const { inline bool Joint::isAlreadyInIsland() const {
return mIsAlreadyInIsland; return mIsAlreadyInIsland;
} }
} }
#endif

1103
ephysics/constraint/SliderJoint.cpp
View File

File diff suppressed because it is too large Load Diff

479
ephysics/constraint/SliderJoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SLIDER_JOINT_H
#define REACTPHYSICS3D_SLIDER_JOINT_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -39,94 +18,94 @@ namespace reactphysics3d {
*/ */
struct SliderJointInfo : public JointInfo { struct SliderJointInfo : public JointInfo {
public : public :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates) /// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace; Vector3 anchorPointWorldSpace;
/// Slider axis (in world-space coordinates) /// Slider axis (in world-space coordinates)
Vector3 sliderAxisWorldSpace; Vector3 sliderAxisWorldSpace;
/// True if the slider limits are enabled /// True if the slider limits are enabled
bool isLimitEnabled; bool isLimitEnabled;
/// True if the slider motor is enabled /// True if the slider motor is enabled
bool isMotorEnabled; bool isMotorEnabled;
/// Mininum allowed translation if limits are enabled /// Mininum allowed translation if limits are enabled
decimal minTranslationLimit; float minTranslationLimit;
/// Maximum allowed translation if limits are enabled /// Maximum allowed translation if limits are enabled
decimal maxTranslationLimit; float maxTranslationLimit;
/// Motor speed /// Motor speed
decimal motorSpeed; float motorSpeed;
/// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal maxMotorForce; float maxMotorForce;
/// Constructor without limits and without motor /// Constructor without limits and without motor
/** /**
* @param rigidBody1 The first body of the joint * @param rigidBody1 The first body of the joint
* @param rigidBody2 The second body of the joint * @param rigidBody2 The second body of the joint
* @param initAnchorPointWorldSpace The initial anchor point in world-space * @param initAnchorPointWorldSpace The initial anchor point in world-space
* @param initSliderAxisWorldSpace The initial slider axis in world-space * @param initSliderAxisWorldSpace The initial slider axis in world-space
*/ */
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace) const Vector3& initSliderAxisWorldSpace)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0), isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0),
maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {} maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {}
/// Constructor with limits and no motor /// Constructor with limits and no motor
/** /**
* @param rigidBody1 The first body of the joint * @param rigidBody1 The first body of the joint
* @param rigidBody2 The second body of the joint * @param rigidBody2 The second body of the joint
* @param initAnchorPointWorldSpace The initial anchor point in world-space * @param initAnchorPointWorldSpace The initial anchor point in world-space
* @param initSliderAxisWorldSpace The initial slider axis in world-space * @param initSliderAxisWorldSpace The initial slider axis in world-space
* @param initMinTranslationLimit The initial minimum translation limit (in meters) * @param initMinTranslationLimit The initial minimum translation limit (in meters)
* @param initMaxTranslationLimit The initial maximum translation limit (in meters) * @param initMaxTranslationLimit The initial maximum translation limit (in meters)
*/ */
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace, const Vector3& initSliderAxisWorldSpace,
decimal initMinTranslationLimit, decimal initMaxTranslationLimit) float initMinTranslationLimit, float initMaxTranslationLimit)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(true), isMotorEnabled(false), isLimitEnabled(true), isMotorEnabled(false),
minTranslationLimit(initMinTranslationLimit), minTranslationLimit(initMinTranslationLimit),
maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0), maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0),
maxMotorForce(0) {} maxMotorForce(0) {}
/// Constructor with limits and motor /// Constructor with limits and motor
/** /**
* @param rigidBody1 The first body of the joint * @param rigidBody1 The first body of the joint
* @param rigidBody2 The second body of the joint * @param rigidBody2 The second body of the joint
* @param initAnchorPointWorldSpace The initial anchor point in world-space * @param initAnchorPointWorldSpace The initial anchor point in world-space
* @param initSliderAxisWorldSpace The initial slider axis in world-space * @param initSliderAxisWorldSpace The initial slider axis in world-space
* @param initMinTranslationLimit The initial minimum translation limit (in meters) * @param initMinTranslationLimit The initial minimum translation limit (in meters)
* @param initMaxTranslationLimit The initial maximum translation limit (in meters) * @param initMaxTranslationLimit The initial maximum translation limit (in meters)
* @param initMotorSpeed The initial speed of the joint motor (in meters per second) * @param initMotorSpeed The initial speed of the joint motor (in meters per second)
* @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters) * @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters)
*/ */
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace, const Vector3& initSliderAxisWorldSpace,
decimal initMinTranslationLimit, decimal initMaxTranslationLimit, float initMinTranslationLimit, float initMaxTranslationLimit,
decimal initMotorSpeed, decimal initMaxMotorForce) float initMotorSpeed, float initMaxMotorForce)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(true), isMotorEnabled(true), isLimitEnabled(true), isMotorEnabled(true),
minTranslationLimit(initMinTranslationLimit), minTranslationLimit(initMinTranslationLimit),
maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed), maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed),
maxMotorForce(initMaxMotorForce) {} maxMotorForce(initMaxMotorForce) {}
}; };
// Class SliderJoint // Class SliderJoint
@ -137,206 +116,206 @@ struct SliderJointInfo : public JointInfo {
*/ */
class SliderJoint : public Joint { class SliderJoint : public Joint {
private : private :
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the position correction bias factor // Beta value for the position correction bias factor
static const decimal BETA; static const float BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1) /// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1; Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2; Vector3 mLocalAnchorPointBody2;
/// Slider axis (in local-space coordinates of body 1) /// Slider axis (in local-space coordinates of body 1)
Vector3 mSliderAxisBody1; Vector3 mSliderAxisBody1;
/// Inertia tensor of body 1 (in world-space coordinates) /// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1; Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates) /// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2; Matrix3x3 mI2;
/// Inverse of the initial orientation difference between the two bodies /// Inverse of the initial orientation difference between the two bodies
Quaternion mInitOrientationDifferenceInv; Quaternion mInitOrientationDifferenceInv;
/// First vector orthogonal to the slider axis local-space of body 1 /// First vector orthogonal to the slider axis local-space of body 1
Vector3 mN1; Vector3 mN1;
/// Second vector orthogonal to the slider axis and mN1 in local-space of body 1 /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1
Vector3 mN2; Vector3 mN2;
/// Vector r1 in world-space coordinates /// Vector r1 in world-space coordinates
Vector3 mR1; Vector3 mR1;
/// Vector r2 in world-space coordinates /// Vector r2 in world-space coordinates
Vector3 mR2; Vector3 mR2;
/// Cross product of r2 and n1 /// Cross product of r2 and n1
Vector3 mR2CrossN1; Vector3 mR2CrossN1;
/// Cross product of r2 and n2 /// Cross product of r2 and n2
Vector3 mR2CrossN2; Vector3 mR2CrossN2;
/// Cross product of r2 and the slider axis /// Cross product of r2 and the slider axis
Vector3 mR2CrossSliderAxis; Vector3 mR2CrossSliderAxis;
/// Cross product of vector (r1 + u) and n1 /// Cross product of vector (r1 + u) and n1
Vector3 mR1PlusUCrossN1; Vector3 mR1PlusUCrossN1;
/// Cross product of vector (r1 + u) and n2 /// Cross product of vector (r1 + u) and n2
Vector3 mR1PlusUCrossN2; Vector3 mR1PlusUCrossN2;
/// Cross product of vector (r1 + u) and the slider axis /// Cross product of vector (r1 + u) and the slider axis
Vector3 mR1PlusUCrossSliderAxis; Vector3 mR1PlusUCrossSliderAxis;
/// Bias of the 2 translation constraints /// Bias of the 2 translation constraints
Vector2 mBTranslation; Vector2 mBTranslation;
/// Bias of the 3 rotation constraints /// Bias of the 3 rotation constraints
Vector3 mBRotation; Vector3 mBRotation;
/// Bias of the lower limit constraint /// Bias of the lower limit constraint
decimal mBLowerLimit; float mBLowerLimit;
/// Bias of the upper limit constraint /// Bias of the upper limit constraint
decimal mBUpperLimit; float mBUpperLimit;
/// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix)
Matrix2x2 mInverseMassMatrixTranslationConstraint; Matrix2x2 mInverseMassMatrixTranslationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix)
Matrix3x3 mInverseMassMatrixRotationConstraint; Matrix3x3 mInverseMassMatrixRotationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
decimal mInverseMassMatrixLimit; float mInverseMassMatrixLimit;
/// Inverse of mass matrix K=JM^-1J^t for the motor /// Inverse of mass matrix K=JM^-1J^t for the motor
decimal mInverseMassMatrixMotor; float mInverseMassMatrixMotor;
/// Accumulated impulse for the 2 translation constraints /// Accumulated impulse for the 2 translation constraints
Vector2 mImpulseTranslation; Vector2 mImpulseTranslation;
/// Accumulated impulse for the 3 rotation constraints /// Accumulated impulse for the 3 rotation constraints
Vector3 mImpulseRotation; Vector3 mImpulseRotation;
/// Accumulated impulse for the lower limit constraint /// Accumulated impulse for the lower limit constraint
decimal mImpulseLowerLimit; float mImpulseLowerLimit;
/// Accumulated impulse for the upper limit constraint /// Accumulated impulse for the upper limit constraint
decimal mImpulseUpperLimit; float mImpulseUpperLimit;
/// Accumulated impulse for the motor /// Accumulated impulse for the motor
decimal mImpulseMotor; float mImpulseMotor;
/// True if the slider limits are enabled /// True if the slider limits are enabled
bool mIsLimitEnabled; bool mIsLimitEnabled;
/// True if the motor of the joint in enabled /// True if the motor of the joint in enabled
bool mIsMotorEnabled; bool mIsMotorEnabled;
/// Slider axis in world-space coordinates /// Slider axis in world-space coordinates
Vector3 mSliderAxisWorld; Vector3 mSliderAxisWorld;
/// Lower limit (minimum translation distance) /// Lower limit (minimum translation distance)
decimal mLowerLimit; float mLowerLimit;
/// Upper limit (maximum translation distance) /// Upper limit (maximum translation distance)
decimal mUpperLimit; float mUpperLimit;
/// True if the lower limit is violated /// True if the lower limit is violated
bool mIsLowerLimitViolated; bool mIsLowerLimitViolated;
/// True if the upper limit is violated /// True if the upper limit is violated
bool mIsUpperLimitViolated; bool mIsUpperLimitViolated;
/// Motor speed (in m/s) /// Motor speed (in m/s)
decimal mMotorSpeed; float mMotorSpeed;
/// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorForce; float mMaxMotorForce;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
SliderJoint(const SliderJoint& constraint); SliderJoint(const SliderJoint& constraint);
/// Private assignment operator /// Private assignment operator
SliderJoint& operator=(const SliderJoint& constraint); SliderJoint& operator=(const SliderJoint& constraint);
/// Reset the limits /// Reset the limits
void resetLimits(); void resetLimits();
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint /// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step) /// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData); virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction) /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
SliderJoint(const SliderJointInfo& jointInfo); SliderJoint(const SliderJointInfo& jointInfo);
/// Destructor /// Destructor
virtual ~SliderJoint(); virtual ~SliderJoint();
/// Return true if the limits or the joint are enabled /// Return true if the limits or the joint are enabled
bool isLimitEnabled() const; bool isLimitEnabled() const;
/// Return true if the motor of the joint is enabled /// Return true if the motor of the joint is enabled
bool isMotorEnabled() const; bool isMotorEnabled() const;
/// Enable/Disable the limits of the joint /// Enable/Disable the limits of the joint
void enableLimit(bool isLimitEnabled); void enableLimit(bool isLimitEnabled);
/// Enable/Disable the motor of the joint /// Enable/Disable the motor of the joint
void enableMotor(bool isMotorEnabled); void enableMotor(bool isMotorEnabled);
/// Return the current translation value of the joint /// Return the current translation value of the joint
decimal getTranslation() const; float getTranslation() const;
/// Return the minimum translation limit /// Return the minimum translation limit
decimal getMinTranslationLimit() const; float getMinTranslationLimit() const;
/// Set the minimum translation limit /// Set the minimum translation limit
void setMinTranslationLimit(decimal lowerLimit); void setMinTranslationLimit(float lowerLimit);
/// Return the maximum translation limit /// Return the maximum translation limit
decimal getMaxTranslationLimit() const; float getMaxTranslationLimit() const;
/// Set the maximum translation limit /// Set the maximum translation limit
void setMaxTranslationLimit(decimal upperLimit); void setMaxTranslationLimit(float upperLimit);
/// Return the motor speed /// Return the motor speed
decimal getMotorSpeed() const; float getMotorSpeed() const;
/// Set the motor speed /// Set the motor speed
void setMotorSpeed(decimal motorSpeed); void setMotorSpeed(float motorSpeed);
/// Return the maximum motor force /// Return the maximum motor force
decimal getMaxMotorForce() const; float getMaxMotorForce() const;
/// Set the maximum motor force /// Set the maximum motor force
void setMaxMotorForce(decimal maxMotorForce); void setMaxMotorForce(float maxMotorForce);
/// Return the intensity of the current force applied for the joint motor /// Return the int32_tensity of the current force applied for the joint motor
decimal getMotorForce(decimal timeStep) const; float getMotorForce(float timeStep) const;
}; };
// Return true if the limits or the joint are enabled // Return true if the limits or the joint are enabled
@ -344,7 +323,7 @@ class SliderJoint : public Joint {
* @return True if the joint limits are enabled * @return True if the joint limits are enabled
*/ */
inline bool SliderJoint::isLimitEnabled() const { inline bool SliderJoint::isLimitEnabled() const {
return mIsLimitEnabled; return mIsLimitEnabled;
} }
// Return true if the motor of the joint is enabled // Return true if the motor of the joint is enabled
@ -352,55 +331,53 @@ inline bool SliderJoint::isLimitEnabled() const {
* @return True if the joint motor is enabled * @return True if the joint motor is enabled
*/ */
inline bool SliderJoint::isMotorEnabled() const { inline bool SliderJoint::isMotorEnabled() const {
return mIsMotorEnabled; return mIsMotorEnabled;
} }
// Return the minimum translation limit // Return the minimum translation limit
/** /**
* @return The minimum translation limit of the joint (in meters) * @return The minimum translation limit of the joint (in meters)
*/ */
inline decimal SliderJoint::getMinTranslationLimit() const { inline float SliderJoint::getMinTranslationLimit() const {
return mLowerLimit; return mLowerLimit;
} }
// Return the maximum translation limit // Return the maximum translation limit
/** /**
* @return The maximum translation limit of the joint (in meters) * @return The maximum translation limit of the joint (in meters)
*/ */
inline decimal SliderJoint::getMaxTranslationLimit() const { inline float SliderJoint::getMaxTranslationLimit() const {
return mUpperLimit; return mUpperLimit;
} }
// Return the motor speed // Return the motor speed
/** /**
* @return The current motor speed of the joint (in meters per second) * @return The current motor speed of the joint (in meters per second)
*/ */
inline decimal SliderJoint::getMotorSpeed() const { inline float SliderJoint::getMotorSpeed() const {
return mMotorSpeed; return mMotorSpeed;
} }
// Return the maximum motor force // Return the maximum motor force
/** /**
* @return The maximum force of the joint motor (in Newton x meters) * @return The maximum force of the joint motor (in Newton x meters)
*/ */
inline decimal SliderJoint::getMaxMotorForce() const { inline float SliderJoint::getMaxMotorForce() const {
return mMaxMotorForce; return mMaxMotorForce;
} }
// Return the intensity of the current force applied for the joint motor // Return the int32_tensity of the current force applied for the joint motor
/** /**
* @param timeStep Time step (in seconds) * @param timeStep Time step (in seconds)
* @return The current force of the joint motor (in Newton x meters) * @return The current force of the joint motor (in Newton x meters)
*/ */
inline decimal SliderJoint::getMotorForce(decimal timeStep) const { inline float SliderJoint::getMotorForce(float timeStep) const {
return mImpulseMotor / timeStep; return mImpulseMotor / timeStep;
} }
// Return the number of bytes used by the joint // Return the number of bytes used by the joint
inline size_t SliderJoint::getSizeInBytes() const { inline size_t SliderJoint::getSizeInBytes() const {
return sizeof(SliderJoint); return sizeof(SliderJoint);
} }
} }
#endif

35
ephysics/decimal.h
View File

@ -1,35 +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_DECIMAL_H
#define REACTPHYSICS3D_DECIMAL_H
/// ReactPhysiscs3D namespace
namespace reactphysics3d {
typedef float decimal;
}
#endif

251
ephysics/engine/CollisionWorld.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/engine/CollisionWorld.h> #include <ephysics/engine/CollisionWorld.h>
@ -33,23 +14,23 @@ using namespace std;
// Constructor // Constructor
CollisionWorld::CollisionWorld() CollisionWorld::CollisionWorld()
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0), : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
mEventListener(NULL) { mEventListener(NULL) {
} }
// Destructor // Destructor
CollisionWorld::~CollisionWorld() { CollisionWorld::~CollisionWorld() {
// Destroy all the collision bodies that have not been removed // Destroy all the collision bodies that have not been removed
std::set<CollisionBody*>::iterator itBodies; std::set<CollisionBody*>::iterator itBodies;
for (itBodies = mBodies.begin(); itBodies != mBodies.end(); ) { for (itBodies = mBodies.begin(); itBodies != mBodies.end(); ) {
std::set<CollisionBody*>::iterator itToRemove = itBodies; std::set<CollisionBody*>::iterator itToRemove = itBodies;
++itBodies; ++itBodies;
destroyCollisionBody(*itToRemove); destroyCollisionBody(*itToRemove);
} }
assert(mBodies.empty()); assert(mBodies.empty());
} }
// Create a collision body and add it to the world // Create a collision body and add it to the world
@ -59,23 +40,23 @@ CollisionWorld::~CollisionWorld() {
*/ */
CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
// Get the next available body ID // Get the next available body ID
bodyindex bodyID = computeNextAvailableBodyID(); bodyindex bodyID = computeNextAvailableBodyID();
// Largest index cannot be used (it is used for invalid index) // Largest index cannot be used (it is used for invalid index)
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max()); assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
// Create the collision body // Create the collision body
CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody))) CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody)))
CollisionBody(transform, *this, bodyID); CollisionBody(transform, *this, bodyID);
assert(collisionBody != NULL); assert(collisionBody != NULL);
// Add the collision body to the world // Add the collision body to the world
mBodies.insert(collisionBody); mBodies.insert(collisionBody);
// Return the pointer to the rigid body // Return the pointer to the rigid body
return collisionBody; return collisionBody;
} }
// Destroy a collision body // Destroy a collision body
@ -84,48 +65,48 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
*/ */
void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
// Remove all the collision shapes of the body // Remove all the collision shapes of the body
collisionBody->removeAllCollisionShapes(); collisionBody->removeAllCollisionShapes();
// Add the body ID to the list of free IDs // Add the body ID to the list of free IDs
mFreeBodiesIDs.push_back(collisionBody->getID()); mFreeBodiesIDs.push_back(collisionBody->getID());
// Call the destructor of the collision body // Call the destructor of the collision body
collisionBody->~CollisionBody(); collisionBody->~CollisionBody();
// Remove the collision body from the list of bodies // Remove the collision body from the list of bodies
mBodies.erase(collisionBody); mBodies.erase(collisionBody);
// Free the object from the memory allocator // Free the object from the memory allocator
mMemoryAllocator.release(collisionBody, sizeof(CollisionBody)); mMemoryAllocator.release(collisionBody, sizeof(CollisionBody));
} }
// Return the next available body ID // Return the next available body ID
bodyindex CollisionWorld::computeNextAvailableBodyID() { bodyindex CollisionWorld::computeNextAvailableBodyID() {
// Compute the body ID // Compute the body ID
bodyindex bodyID; bodyindex bodyID;
if (!mFreeBodiesIDs.empty()) { if (!mFreeBodiesIDs.empty()) {
bodyID = mFreeBodiesIDs.back(); bodyID = mFreeBodiesIDs.back();
mFreeBodiesIDs.pop_back(); mFreeBodiesIDs.pop_back();
} }
else { else {
bodyID = mCurrentBodyID; bodyID = mCurrentBodyID;
mCurrentBodyID++; mCurrentBodyID++;
} }
return bodyID; return bodyID;
} }
// Reset all the contact manifolds linked list of each body // Reset all the contact manifolds linked list of each body
void CollisionWorld::resetContactManifoldListsOfBodies() { void CollisionWorld::resetContactManifoldListsOfBodies() {
// For each rigid body of the world // For each rigid body of the world
for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) { for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
// Reset the contact manifold list of the body // Reset the contact manifold list of the body
(*it)->resetContactManifoldsList(); (*it)->resetContactManifoldsList();
} }
} }
// Test if the AABBs of two bodies overlap // Test if the AABBs of two bodies overlap
@ -135,17 +116,17 @@ void CollisionWorld::resetContactManifoldListsOfBodies() {
* @return True if the AABBs of the two bodies overlap and false otherwise * @return True if the AABBs of the two bodies overlap and false otherwise
*/ */
bool CollisionWorld::testAABBOverlap(const CollisionBody* body1, bool CollisionWorld::testAABBOverlap(const CollisionBody* body1,
const CollisionBody* body2) const { const CollisionBody* body2) const {
// If one of the body is not active, we return no overlap // If one of the body is not active, we return no overlap
if (!body1->isActive() || !body2->isActive()) return false; if (!body1->isActive() || !body2->isActive()) return false;
// Compute the AABBs of both bodies // Compute the AABBs of both bodies
AABB body1AABB = body1->getAABB(); AABB body1AABB = body1->getAABB();
AABB body2AABB = body2->getAABB(); AABB body2AABB = body2->getAABB();
// Return true if the two AABBs overlap // Return true if the two AABBs overlap
return body1AABB.testCollision(body2AABB); return body1AABB.testCollision(body2AABB);
} }
// Test and report collisions between a given shape and all the others // Test and report collisions between a given shape and all the others
@ -155,18 +136,18 @@ bool CollisionWorld::testAABBOverlap(const CollisionBody* body1,
* @param callback Pointer to the object with the callback method * @param callback Pointer to the object with the callback method
*/ */
void CollisionWorld::testCollision(const ProxyShape* shape, void CollisionWorld::testCollision(const ProxyShape* shape,
CollisionCallback* callback) { CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes; std::set<uint32_t> shapes;
shapes.insert(shape->mBroadPhaseID); shapes.insert(shape->mBroadPhaseID);
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
} }
// Test and report collisions between two given shapes // Test and report collisions between two given shapes
@ -176,20 +157,20 @@ void CollisionWorld::testCollision(const ProxyShape* shape,
* @param callback Pointer to the object with the callback method * @param callback Pointer to the object with the callback method
*/ */
void CollisionWorld::testCollision(const ProxyShape* shape1, void CollisionWorld::testCollision(const ProxyShape* shape1,
const ProxyShape* shape2, const ProxyShape* shape2,
CollisionCallback* callback) { CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
shapes1.insert(shape1->mBroadPhaseID); shapes1.insert(shape1->mBroadPhaseID);
std::set<uint> shapes2; std::set<uint32_t> shapes2;
shapes2.insert(shape2->mBroadPhaseID); shapes2.insert(shape2->mBroadPhaseID);
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
} }
// Test and report collisions between a body and all the others bodies of the // Test and report collisions between a body and all the others bodies of the
@ -199,24 +180,24 @@ void CollisionWorld::testCollision(const ProxyShape* shape1,
* @param callback Pointer to the object with the callback method * @param callback Pointer to the object with the callback method
*/ */
void CollisionWorld::testCollision(const CollisionBody* body, void CollisionWorld::testCollision(const CollisionBody* body,
CollisionCallback* callback) { CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
// For each shape of the body // For each shape of the body
for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes1.insert(shape->mBroadPhaseID); shapes1.insert(shape->mBroadPhaseID);
} }
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
} }
// Test and report collisions between two bodies // Test and report collisions between two bodies
@ -226,27 +207,27 @@ void CollisionWorld::testCollision(const CollisionBody* body,
* @param callback Pointer to the object with the callback method * @param callback Pointer to the object with the callback method
*/ */
void CollisionWorld::testCollision(const CollisionBody* body1, void CollisionWorld::testCollision(const CollisionBody* body1,
const CollisionBody* body2, const CollisionBody* body2,
CollisionCallback* callback) { CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes1.insert(shape->mBroadPhaseID); shapes1.insert(shape->mBroadPhaseID);
} }
std::set<uint> shapes2; std::set<uint32_t> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes2.insert(shape->mBroadPhaseID); shapes2.insert(shape->mBroadPhaseID);
} }
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
} }
// Test and report collisions between all shapes of the world // Test and report collisions between all shapes of the world
@ -255,12 +236,12 @@ void CollisionWorld::testCollision(const CollisionBody* body1,
*/ */
void CollisionWorld::testCollision(CollisionCallback* callback) { void CollisionWorld::testCollision(CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
} }

207
ephysics/engine/CollisionWorld.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_WORLD_H
#define REACTPHYSICS3D_COLLISION_WORLD_H
// Libraries // Libraries
#include <vector> #include <vector>
@ -56,108 +35,108 @@ class CollisionCallback;
*/ */
class CollisionWorld { class CollisionWorld {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Reference to the collision detection /// Reference to the collision detection
CollisionDetection mCollisionDetection; CollisionDetection mCollisionDetection;
/// All the bodies (rigid and soft) of the world /// All the bodies (rigid and soft) of the world
std::set<CollisionBody*> mBodies; std::set<CollisionBody*> mBodies;
/// Current body ID /// Current body ID
bodyindex mCurrentBodyID; bodyindex mCurrentBodyID;
/// List of free ID for rigid bodies /// List of free ID for rigid bodies
std::vector<luint> mFreeBodiesIDs; std::vector<uint64_t> mFreeBodiesIDs;
/// Memory allocator /// Memory allocator
MemoryAllocator mMemoryAllocator; MemoryAllocator mMemoryAllocator;
/// Pointer to an event listener object /// Pointer to an event listener object
EventListener* mEventListener; EventListener* mEventListener;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
CollisionWorld(const CollisionWorld& world); CollisionWorld(const CollisionWorld& world);
/// Private assignment operator /// Private assignment operator
CollisionWorld& operator=(const CollisionWorld& world); CollisionWorld& operator=(const CollisionWorld& world);
/// Return the next available body ID /// Return the next available body ID
bodyindex computeNextAvailableBodyID(); bodyindex computeNextAvailableBodyID();
/// Reset all the contact manifolds linked list of each body /// Reset all the contact manifolds linked list of each body
void resetContactManifoldListsOfBodies(); void resetContactManifoldListsOfBodies();
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
CollisionWorld(); CollisionWorld();
/// Destructor /// Destructor
virtual ~CollisionWorld(); virtual ~CollisionWorld();
/// Return an iterator to the beginning of the bodies of the physics world /// Return an iterator to the beginning of the bodies of the physics world
std::set<CollisionBody*>::iterator getBodiesBeginIterator(); std::set<CollisionBody*>::iterator getBodiesBeginIterator();
/// Return an iterator to the end of the bodies of the physics world /// Return an iterator to the end of the bodies of the physics world
std::set<CollisionBody*>::iterator getBodiesEndIterator(); std::set<CollisionBody*>::iterator getBodiesEndIterator();
/// Create a collision body /// Create a collision body
CollisionBody* createCollisionBody(const Transform& transform); CollisionBody* createCollisionBody(const Transform& transform);
/// Destroy a collision body /// Destroy a collision body
void destroyCollisionBody(CollisionBody* collisionBody); void destroyCollisionBody(CollisionBody* collisionBody);
/// Set the collision dispatch configuration /// Set the collision dispatch configuration
void setCollisionDispatch(CollisionDispatch* collisionDispatch); void setCollisionDispatch(CollisionDispatch* collisionDispatch);
/// Ray cast method /// Ray cast method
void raycast(const Ray& ray, RaycastCallback* raycastCallback, void raycast(const Ray& ray, RaycastCallback* raycastCallback,
unsigned short raycastWithCategoryMaskBits = 0xFFFF) const; unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
/// Test if the AABBs of two bodies overlap /// Test if the AABBs of two bodies overlap
bool testAABBOverlap(const CollisionBody* body1, bool testAABBOverlap(const CollisionBody* body1,
const CollisionBody* body2) const; const CollisionBody* body2) const;
/// Test if the AABBs of two proxy shapes overlap /// Test if the AABBs of two proxy shapes overlap
bool testAABBOverlap(const ProxyShape* shape1, bool testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const; const ProxyShape* shape2) const;
/// Test and report collisions between a given shape and all the others /// Test and report collisions between a given shape and all the others
/// shapes of the world /// shapes of the world
virtual void testCollision(const ProxyShape* shape, virtual void testCollision(const ProxyShape* shape,
CollisionCallback* callback); CollisionCallback* callback);
/// Test and report collisions between two given shapes /// Test and report collisions between two given shapes
virtual void testCollision(const ProxyShape* shape1, virtual void testCollision(const ProxyShape* shape1,
const ProxyShape* shape2, const ProxyShape* shape2,
CollisionCallback* callback); CollisionCallback* callback);
/// Test and report collisions between a body and all the others bodies of the /// Test and report collisions between a body and all the others bodies of the
/// world /// world
virtual void testCollision(const CollisionBody* body, virtual void testCollision(const CollisionBody* body,
CollisionCallback* callback); CollisionCallback* callback);
/// Test and report collisions between two bodies /// Test and report collisions between two bodies
virtual void testCollision(const CollisionBody* body1, virtual void testCollision(const CollisionBody* body1,
const CollisionBody* body2, const CollisionBody* body2,
CollisionCallback* callback); CollisionCallback* callback);
/// Test and report collisions between all shapes of the world /// Test and report collisions between all shapes of the world
virtual void testCollision(CollisionCallback* callback); virtual void testCollision(CollisionCallback* callback);
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class CollisionDetection; friend class CollisionDetection;
friend class CollisionBody; friend class CollisionBody;
friend class RigidBody; friend class RigidBody;
friend class ConvexMeshShape; friend class ConvexMeshShape;
}; };
// Return an iterator to the beginning of the bodies of the physics world // Return an iterator to the beginning of the bodies of the physics world
@ -165,7 +144,7 @@ class CollisionWorld {
* @return An starting iterator to the set of bodies of the world * @return An starting iterator to the set of bodies of the world
*/ */
inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesBeginIterator() { inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesBeginIterator() {
return mBodies.begin(); return mBodies.begin();
} }
// Return an iterator to the end of the bodies of the physics world // Return an iterator to the end of the bodies of the physics world
@ -173,7 +152,7 @@ inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesBeginIterator
* @return An ending iterator to the set of bodies of the world * @return An ending iterator to the set of bodies of the world
*/ */
inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator() { inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator() {
return mBodies.end(); return mBodies.end();
} }
// Set the collision dispatch configuration // Set the collision dispatch configuration
@ -184,7 +163,7 @@ inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator()
* which collision detection algorithm to use for two given collision shapes * which collision detection algorithm to use for two given collision shapes
*/ */
inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) { inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDetection.setCollisionDispatch(collisionDispatch); mCollisionDetection.setCollisionDispatch(collisionDispatch);
} }
// Ray cast method // Ray cast method
@ -192,12 +171,12 @@ inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDis
* @param ray Ray to use for raycasting * @param ray Ray to use for raycasting
* @param raycastCallback Pointer to the class with the callback method * @param raycastCallback Pointer to the class with the callback method
* @param raycastWithCategoryMaskBits Bits mask corresponding to the category of * @param raycastWithCategoryMaskBits Bits mask corresponding to the category of
* bodies to be raycasted * bodies to be raycasted
*/ */
inline void CollisionWorld::raycast(const Ray& ray, inline void CollisionWorld::raycast(const Ray& ray,
RaycastCallback* raycastCallback, RaycastCallback* raycastCallback,
unsigned short raycastWithCategoryMaskBits) const { unsigned short raycastWithCategoryMaskBits) const {
mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits); mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
} }
// Test if the AABBs of two proxy shapes overlap // Test if the AABBs of two proxy shapes overlap
@ -207,9 +186,9 @@ inline void CollisionWorld::raycast(const Ray& ray,
* @return * @return
*/ */
inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1, inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const { const ProxyShape* shape2) const {
return mCollisionDetection.testAABBOverlap(shape1, shape2); return mCollisionDetection.testAABBOverlap(shape1, shape2);
} }
// Class CollisionCallback // Class CollisionCallback
@ -221,17 +200,15 @@ inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1,
*/ */
class CollisionCallback { class CollisionCallback {
public: public:
/// Destructor /// Destructor
virtual ~CollisionCallback() { virtual ~CollisionCallback() {
} }
/// This method will be called for contact /// This method will be called for contact
virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0; virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0;
}; };
} }
#endif

111
ephysics/engine/ConstraintSolver.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/engine/ConstraintSolver.h> #include <ephysics/engine/ConstraintSolver.h>
@ -30,9 +11,9 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex) ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex)
: mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) { mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
} }
@ -42,65 +23,65 @@ ConstraintSolver::~ConstraintSolver() {
} }
// Initialize the constraint solver for a given island // Initialize the constraint solver for a given island
void ConstraintSolver::initializeForIsland(decimal dt, Island* island) { void ConstraintSolver::initializeForIsland(float dt, Island* island) {
PROFILE("ConstraintSolver::initializeForIsland()"); PROFILE("ConstraintSolver::initializeForIsland()");
assert(island != NULL); assert(island != NULL);
assert(island->getNbBodies() > 0); assert(island->getNbBodies() > 0);
assert(island->getNbJoints() > 0); assert(island->getNbJoints() > 0);
// Set the current time step // Set the current time step
mTimeStep = dt; mTimeStep = dt;
// Initialize the constraint solver data used to initialize and solve the constraints // Initialize the constraint solver data used to initialize and solve the constraints
mConstraintSolverData.timeStep = mTimeStep; mConstraintSolverData.timeStep = mTimeStep;
mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive; mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
// For each joint of the island // For each joint of the island
Joint** joints = island->getJoints(); Joint** joints = island->getJoints();
for (uint i=0; i<island->getNbJoints(); i++) { for (uint32_t i=0; i<island->getNbJoints(); i++) {
// Initialize the constraint before solving it // Initialize the constraint before solving it
joints[i]->initBeforeSolve(mConstraintSolverData); joints[i]->initBeforeSolve(mConstraintSolverData);
// Warm-start the constraint if warm-starting is enabled // Warm-start the constraint if warm-starting is enabled
if (mIsWarmStartingActive) { if (mIsWarmStartingActive) {
joints[i]->warmstart(mConstraintSolverData); joints[i]->warmstart(mConstraintSolverData);
} }
} }
} }
// Solve the velocity constraints // Solve the velocity constraints
void ConstraintSolver::solveVelocityConstraints(Island* island) { void ConstraintSolver::solveVelocityConstraints(Island* island) {
PROFILE("ConstraintSolver::solveVelocityConstraints()"); PROFILE("ConstraintSolver::solveVelocityConstraints()");
assert(island != NULL); assert(island != NULL);
assert(island->getNbJoints() > 0); assert(island->getNbJoints() > 0);
// For each joint of the island // For each joint of the island
Joint** joints = island->getJoints(); Joint** joints = island->getJoints();
for (uint i=0; i<island->getNbJoints(); i++) { for (uint32_t i=0; i<island->getNbJoints(); i++) {
// Solve the constraint // Solve the constraint
joints[i]->solveVelocityConstraint(mConstraintSolverData); joints[i]->solveVelocityConstraint(mConstraintSolverData);
} }
} }
// Solve the position constraints // Solve the position constraints
void ConstraintSolver::solvePositionConstraints(Island* island) { void ConstraintSolver::solvePositionConstraints(Island* island) {
PROFILE("ConstraintSolver::solvePositionConstraints()"); PROFILE("ConstraintSolver::solvePositionConstraints()");
assert(island != NULL); assert(island != NULL);
assert(island->getNbJoints() > 0); assert(island->getNbJoints() > 0);
// For each joint of the island // For each joint of the island
Joint** joints = island->getJoints(); Joint** joints = island->getJoints();
for (uint i=0; i < island->getNbJoints(); i++) { for (uint32_t i=0; i < island->getNbJoints(); i++) {
// Solve the constraint // Solve the constraint
joints[i]->solvePositionConstraint(mConstraintSolverData); joints[i]->solvePositionConstraint(mConstraintSolverData);
} }
} }

169
ephysics/engine/ConstraintSolver.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONSTRAINT_SOLVER_H
#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
// Libraries // Libraries
#include <ephysics/configuration.h> #include <ephysics/configuration.h>
@ -43,37 +22,37 @@ namespace reactphysics3d {
*/ */
struct ConstraintSolverData { struct ConstraintSolverData {
public : public :
/// Current time step of the simulation /// Current time step of the simulation
decimal timeStep; float timeStep;
/// Array with the bodies linear velocities /// Array with the bodies linear velocities
Vector3* linearVelocities; Vector3* linearVelocities;
/// Array with the bodies angular velocities /// Array with the bodies angular velocities
Vector3* angularVelocities; Vector3* angularVelocities;
/// Reference to the bodies positions /// Reference to the bodies positions
Vector3* positions; Vector3* positions;
/// Reference to the bodies orientations /// Reference to the bodies orientations
Quaternion* orientations; Quaternion* orientations;
/// Reference to the map that associates rigid body to their index /// Reference to the map that associates rigid body to their index
/// in the constrained velocities array /// in the constrained velocities array
const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex; const std::map<RigidBody*, uint32_t>& mapBodyToConstrainedVelocityIndex;
/// True if warm starting of the solver is active /// True if warm starting of the solver is active
bool isWarmStartingActive; bool isWarmStartingActive;
/// Constructor /// Constructor
ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex) ConstraintSolverData(const std::map<RigidBody*, uint32_t>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL), angularVelocities(NULL), :linearVelocities(NULL), angularVelocities(NULL),
positions(NULL), orientations(NULL), positions(NULL), orientations(NULL),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
} }
}; };
@ -99,7 +78,7 @@ struct ConstraintSolverData {
* *
* --- Step 1 --- * --- Step 1 ---
* *
* First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints. * we obtain some new velocities v2' that tends to violate the constraints.
* *
* v2' = v1 + dt * M^-1 * F_a * v2' = v1 + dt * M^-1 * F_a
@ -120,7 +99,7 @@ struct ConstraintSolverData {
* *
* --- Step 3 --- * --- Step 3 ---
* *
* In the third step, we integrate the new position x2 of the bodies using the new velocities * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2. * v2 computed in the second step with : x2 = x1 + dt * v2.
* *
* Note that in the following code (as it is also explained in the slides from Erin Catto), * Note that in the following code (as it is also explained in the slides from Erin Catto),
@ -148,75 +127,73 @@ struct ConstraintSolverData {
*/ */
class ConstraintSolver { class ConstraintSolver {
private : private :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Reference to the map that associates rigid body to their index in /// Reference to the map that associates rigid body to their index in
/// the constrained velocities array /// the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex; const std::map<RigidBody*, uint32_t>& mMapBodyToConstrainedVelocityIndex;
/// Current time step /// Current time step
decimal mTimeStep; float mTimeStep;
/// True if the warm starting of the solver is active /// True if the warm starting of the solver is active
bool mIsWarmStartingActive; bool mIsWarmStartingActive;
/// Constraint solver data used to initialize and solve the constraints /// Constraint solver data used to initialize and solve the constraints
ConstraintSolverData mConstraintSolverData; ConstraintSolverData mConstraintSolverData;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex); ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor /// Destructor
~ConstraintSolver(); ~ConstraintSolver();
/// Initialize the constraint solver for a given island /// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island); void initializeForIsland(float dt, Island* island);
/// Solve the constraints /// Solve the constraints
void solveVelocityConstraints(Island* island); void solveVelocityConstraints(Island* island);
/// Solve the position constraints /// Solve the position constraints
void solvePositionConstraints(Island* island); void solvePositionConstraints(Island* island);
/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
bool getIsNonLinearGaussSeidelPositionCorrectionActive() const; bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Set the constrained velocities arrays /// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities); Vector3* constrainedAngularVelocities);
/// Set the constrained positions/orientations arrays /// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(Vector3* constrainedPositions, void setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations); Quaternion* constrainedOrientations);
}; };
// Set the constrained velocities arrays // Set the constrained velocities arrays
inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities) { Vector3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL); assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL); assert(constrainedAngularVelocities != NULL);
mConstraintSolverData.linearVelocities = constrainedLinearVelocities; mConstraintSolverData.linearVelocities = constrainedLinearVelocities;
mConstraintSolverData.angularVelocities = constrainedAngularVelocities; mConstraintSolverData.angularVelocities = constrainedAngularVelocities;
} }
// Set the constrained positions/orientations arrays // Set the constrained positions/orientations arrays
inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions, inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations) { Quaternion* constrainedOrientations) {
assert(constrainedPositions != NULL); assert(constrainedPositions != NULL);
assert(constrainedOrientations != NULL); assert(constrainedOrientations != NULL);
mConstraintSolverData.positions = constrainedPositions; mConstraintSolverData.positions = constrainedPositions;
mConstraintSolverData.orientations = constrainedOrientations; mConstraintSolverData.orientations = constrainedOrientations;
} }
} }
#endif

1335
ephysics/engine/ContactSolver.cpp
View File

File diff suppressed because it is too large Load Diff

593
ephysics/engine/ContactSolver.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SOLVER_H
#define REACTPHYSICS3D_CONTACT_SOLVER_H
// Libraries // Libraries
#include <ephysics/constraint/ContactPoint.h> #include <ephysics/constraint/ContactPoint.h>
@ -62,7 +41,7 @@ namespace reactphysics3d {
* *
* --- Step 1 --- * --- Step 1 ---
* *
* First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints. * we obtain some new velocities v2' that tends to violate the constraints.
* *
* v2' = v1 + dt * M^-1 * F_a * v2' = v1 + dt * M^-1 * F_a
@ -83,7 +62,7 @@ namespace reactphysics3d {
* *
* --- Step 3 --- * --- Step 3 ---
* *
* In the third step, we integrate the new position x2 of the bodies using the new velocities * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2. * v2 computed in the second step with : x2 = x1 + dt * v2.
* *
* Note that in the following code (as it is also explained in the slides from Erin Catto), * Note that in the following code (as it is also explained in the slides from Erin Catto),
@ -111,431 +90,429 @@ namespace reactphysics3d {
*/ */
class ContactSolver { class ContactSolver {
private: private:
// Structure ContactPointSolver // Structure ContactPointSolver
/** /**
* Contact solver internal data structure that to store all the * Contact solver int32_ternal data structure that to store all the
* information relative to a contact point * information relative to a contact point
*/ */
struct ContactPointSolver { struct ContactPointSolver {
/// Accumulated normal impulse /// Accumulated normal impulse
decimal penetrationImpulse; float penetrationImpulse;
/// Accumulated impulse in the 1st friction direction /// Accumulated impulse in the 1st friction direction
decimal friction1Impulse; float friction1Impulse;
/// Accumulated impulse in the 2nd friction direction /// Accumulated impulse in the 2nd friction direction
decimal friction2Impulse; float friction2Impulse;
/// Accumulated split impulse for penetration correction /// Accumulated split impulse for penetration correction
decimal penetrationSplitImpulse; float penetrationSplitImpulse;
/// Accumulated rolling resistance impulse /// Accumulated rolling resistance impulse
Vector3 rollingResistanceImpulse; Vector3 rollingResistanceImpulse;
/// Normal vector of the contact /// Normal vector of the contact
Vector3 normal; Vector3 normal;
/// First friction vector in the tangent plane /// First friction vector in the tangent plane
Vector3 frictionVector1; Vector3 frictionVector1;
/// Second friction vector in the tangent plane /// Second friction vector in the tangent plane
Vector3 frictionVector2; Vector3 frictionVector2;
/// Old first friction vector in the tangent plane /// Old first friction vector in the tangent plane
Vector3 oldFrictionVector1; Vector3 oldFrictionVector1;
/// Old second friction vector in the tangent plane /// Old second friction vector in the tangent plane
Vector3 oldFrictionVector2; Vector3 oldFrictionVector2;
/// Vector from the body 1 center to the contact point /// Vector from the body 1 center to the contact point
Vector3 r1; Vector3 r1;
/// Vector from the body 2 center to the contact point /// Vector from the body 2 center to the contact point
Vector3 r2; Vector3 r2;
/// Cross product of r1 with 1st friction vector /// Cross product of r1 with 1st friction vector
Vector3 r1CrossT1; Vector3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector /// Cross product of r1 with 2nd friction vector
Vector3 r1CrossT2; Vector3 r1CrossT2;
/// Cross product of r2 with 1st friction vector /// Cross product of r2 with 1st friction vector
Vector3 r2CrossT1; Vector3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector /// Cross product of r2 with 2nd friction vector
Vector3 r2CrossT2; Vector3 r2CrossT2;
/// Cross product of r1 with the contact normal /// Cross product of r1 with the contact normal
Vector3 r1CrossN; Vector3 r1CrossN;
/// Cross product of r2 with the contact normal /// Cross product of r2 with the contact normal
Vector3 r2CrossN; Vector3 r2CrossN;
/// Penetration depth /// Penetration depth
decimal penetrationDepth; float penetrationDepth;
/// Velocity restitution bias /// Velocity restitution bias
decimal restitutionBias; float restitutionBias;
/// Inverse of the matrix K for the penenetration /// Inverse of the matrix K for the penenetration
decimal inversePenetrationMass; float inversePenetrationMass;
/// Inverse of the matrix K for the 1st friction /// Inverse of the matrix K for the 1st friction
decimal inverseFriction1Mass; float inverseFriction1Mass;
/// Inverse of the matrix K for the 2nd friction /// Inverse of the matrix K for the 2nd friction
decimal inverseFriction2Mass; float inverseFriction2Mass;
/// True if the contact was existing last time step /// True if the contact was existing last time step
bool isRestingContact; bool isRestingContact;
/// Pointer to the external contact /// Pointer to the external contact
ContactPoint* externalContact; ContactPoint* externalContact;
}; };
// Structure ContactManifoldSolver // Structure ContactManifoldSolver
/** /**
* Contact solver internal data structure to store all the * Contact solver int32_ternal data structure to store all the
* information relative to a contact manifold. * information relative to a contact manifold.
*/ */
struct ContactManifoldSolver { struct ContactManifoldSolver {
/// Index of body 1 in the constraint solver /// Index of body 1 in the constraint solver
uint indexBody1; uint32_t indexBody1;
/// Index of body 2 in the constraint solver /// Index of body 2 in the constraint solver
uint indexBody2; uint32_t indexBody2;
/// Inverse of the mass of body 1 /// Inverse of the mass of body 1
decimal massInverseBody1; float massInverseBody1;
// Inverse of the mass of body 2 // Inverse of the mass of body 2
decimal massInverseBody2; float massInverseBody2;
/// Inverse inertia tensor of body 1 /// Inverse inertia tensor of body 1
Matrix3x3 inverseInertiaTensorBody1; Matrix3x3 inverseInertiaTensorBody1;
/// Inverse inertia tensor of body 2 /// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2; Matrix3x3 inverseInertiaTensorBody2;
/// Contact point constraints /// Contact point constraints
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Number of contact points /// Number of contact points
uint nbContacts; uint32_t nbContacts;
/// True if the body 1 is of type dynamic /// True if the body 1 is of type dynamic
bool isBody1DynamicType; bool isBody1DynamicType;
/// True if the body 2 is of type dynamic /// True if the body 2 is of type dynamic
bool isBody2DynamicType; bool isBody2DynamicType;
/// Mix of the restitution factor for two bodies /// Mix of the restitution factor for two bodies
decimal restitutionFactor; float restitutionFactor;
/// Mix friction coefficient for the two bodies /// Mix friction coefficient for the two bodies
decimal frictionCoefficient; float frictionCoefficient;
/// Rolling resistance factor between the two bodies /// Rolling resistance factor between the two bodies
decimal rollingResistanceFactor; float rollingResistanceFactor;
/// Pointer to the external contact manifold /// Pointer to the external contact manifold
ContactManifold* externalContactManifold; ContactManifold* externalContactManifold;
// - Variables used when friction constraints are apply at the center of the manifold-// // - Variables used when friction constraints are apply at the center of the manifold-//
/// Average normal vector of the contact manifold /// Average normal vector of the contact manifold
Vector3 normal; Vector3 normal;
/// Point on body 1 where to apply the friction constraints /// Point on body 1 where to apply the friction constraints
Vector3 frictionPointBody1; Vector3 frictionPointBody1;
/// Point on body 2 where to apply the friction constraints /// Point on body 2 where to apply the friction constraints
Vector3 frictionPointBody2; Vector3 frictionPointBody2;
/// R1 vector for the friction constraints /// R1 vector for the friction constraints
Vector3 r1Friction; Vector3 r1Friction;
/// R2 vector for the friction constraints /// R2 vector for the friction constraints
Vector3 r2Friction; Vector3 r2Friction;
/// Cross product of r1 with 1st friction vector /// Cross product of r1 with 1st friction vector
Vector3 r1CrossT1; Vector3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector /// Cross product of r1 with 2nd friction vector
Vector3 r1CrossT2; Vector3 r1CrossT2;
/// Cross product of r2 with 1st friction vector /// Cross product of r2 with 1st friction vector
Vector3 r2CrossT1; Vector3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector /// Cross product of r2 with 2nd friction vector
Vector3 r2CrossT2; Vector3 r2CrossT2;
/// Matrix K for the first friction constraint /// Matrix K for the first friction constraint
decimal inverseFriction1Mass; float inverseFriction1Mass;
/// Matrix K for the second friction constraint /// Matrix K for the second friction constraint
decimal inverseFriction2Mass; float inverseFriction2Mass;
/// Matrix K for the twist friction constraint /// Matrix K for the twist friction constraint
decimal inverseTwistFrictionMass; float inverseTwistFrictionMass;
/// Matrix K for the rolling resistance constraint /// Matrix K for the rolling resistance constraint
Matrix3x3 inverseRollingResistance; Matrix3x3 inverseRollingResistance;
/// First friction direction at contact manifold center /// First friction direction at contact manifold center
Vector3 frictionVector1; Vector3 frictionVector1;
/// Second friction direction at contact manifold center /// Second friction direction at contact manifold center
Vector3 frictionVector2; Vector3 frictionVector2;
/// Old 1st friction direction at contact manifold center /// Old 1st friction direction at contact manifold center
Vector3 oldFrictionVector1; Vector3 oldFrictionVector1;
/// Old 2nd friction direction at contact manifold center /// Old 2nd friction direction at contact manifold center
Vector3 oldFrictionVector2; Vector3 oldFrictionVector2;
/// First friction direction impulse at manifold center /// First friction direction impulse at manifold center
decimal friction1Impulse; float friction1Impulse;
/// Second friction direction impulse at manifold center /// Second friction direction impulse at manifold center
decimal friction2Impulse; float friction2Impulse;
/// Twist friction impulse at contact manifold center /// Twist friction impulse at contact manifold center
decimal frictionTwistImpulse; float frictionTwistImpulse;
/// Rolling resistance impulse /// Rolling resistance impulse
Vector3 rollingResistanceImpulse; Vector3 rollingResistanceImpulse;
}; };
// -------------------- Constants --------------------- // // -------------------- Constants --------------------- //
/// Beta value for the penetration depth position correction without split impulses /// Beta value for the penetration depth position correction without split impulses
static const decimal BETA; static const float BETA;
/// Beta value for the penetration depth position correction with split impulses /// Beta value for the penetration depth position correction with split impulses
static const decimal BETA_SPLIT_IMPULSE; static const float BETA_SPLIT_IMPULSE;
/// Slop distance (allowed penetration distance between bodies) /// Slop distance (allowed penetration distance between bodies)
static const decimal SLOP; static const float SLOP;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Split linear velocities for the position contact solver (split impulse) /// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities; Vector3* mSplitLinearVelocities;
/// Split angular velocities for the position contact solver (split impulse) /// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities; Vector3* mSplitAngularVelocities;
/// Current time step /// Current time step
decimal mTimeStep; float mTimeStep;
/// Contact constraints /// Contact constraints
ContactManifoldSolver* mContactConstraints; ContactManifoldSolver* mContactConstraints;
/// Number of contact constraints /// Number of contact constraints
uint mNbContactManifolds; uint32_t mNbContactManifolds;
/// Array of linear velocities /// Array of linear velocities
Vector3* mLinearVelocities; Vector3* mLinearVelocities;
/// Array of angular velocities /// Array of angular velocities
Vector3* mAngularVelocities; Vector3* mAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array /// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex; const std::map<RigidBody*, uint32_t>& mMapBodyToConstrainedVelocityIndex;
/// True if the warm starting of the solver is active /// True if the warm starting of the solver is active
bool mIsWarmStartingActive; bool mIsWarmStartingActive;
/// True if the split impulse position correction is active /// True if the split impulse position correction is active
bool mIsSplitImpulseActive; bool mIsSplitImpulseActive;
/// True if we solve 3 friction constraints at the contact manifold center only /// True if we solve 3 friction constraints at the contact manifold center only
/// instead of 2 friction constraints at each contact point /// instead of 2 friction constraints at each contact point
bool mIsSolveFrictionAtContactManifoldCenterActive; bool mIsSolveFrictionAtContactManifoldCenterActive;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Initialize the contact constraints before solving the system /// Initialize the contact constraints before solving the system
void initializeContactConstraints(); void initializeContactConstraints();
/// Apply an impulse to the two bodies of a constraint /// Apply an impulse to the two bodies of a constraint
void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
/// Apply an impulse to the two bodies of a constraint /// Apply an impulse to the two bodies of a constraint
void applySplitImpulse(const Impulse& impulse, void applySplitImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold); const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body /// Compute the collision restitution factor from the restitution factor of each body
decimal computeMixedRestitutionFactor(RigidBody *body1, float computeMixedRestitutionFactor(RigidBody *body1,
RigidBody *body2) const; RigidBody *body2) const;
/// Compute the mixed friction coefficient from the friction coefficient of each body /// Compute the mixed friction coefficient from the friction coefficient of each body
decimal computeMixedFrictionCoefficient(RigidBody* body1, float computeMixedFrictionCoefficient(RigidBody* body1,
RigidBody* body2) const; RigidBody* body2) const;
/// Compute th mixed rolling resistance factor between two bodies /// Compute th mixed rolling resistance factor between two bodies
decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact point. The two vectors have to be /// plane for a contact point. The two vectors have to be
/// such that : t1 x t2 = contactNormal. /// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const Vector3& deltaVelocity, void computeFrictionVectors(const Vector3& deltaVelocity,
ContactPointSolver &contactPoint) const; ContactPointSolver &contactPoint) const;
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact manifold. The two vectors have to be /// plane for a contact manifold. The two vectors have to be
/// such that : t1 x t2 = contactNormal. /// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const Vector3& deltaVelocity, void computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contactPoint) const; ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse /// Compute a penetration constraint impulse
const Impulse computePenetrationImpulse(decimal deltaLambda, const Impulse computePenetrationImpulse(float deltaLambda,
const ContactPointSolver& contactPoint) const; const ContactPointSolver& contactPoint) const;
/// Compute the first friction constraint impulse /// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(decimal deltaLambda, const Impulse computeFriction1Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const; const ContactPointSolver& contactPoint) const;
/// Compute the second friction constraint impulse /// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(decimal deltaLambda, const Impulse computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const; const ContactPointSolver& contactPoint) const;
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex); ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor /// Destructor
virtual ~ContactSolver(); virtual ~ContactSolver();
/// Initialize the constraint solver for a given island /// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island); void initializeForIsland(float dt, Island* island);
/// Set the split velocities arrays /// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
Vector3* splitAngularVelocities); Vector3* splitAngularVelocities);
/// Set the constrained velocities arrays /// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities); Vector3* constrainedAngularVelocities);
/// Warm start the solver. /// Warm start the solver.
void warmStart(); void warmStart();
/// Store the computed impulses to use them to /// Store the computed impulses to use them to
/// warm start the solver at the next iteration /// warm start the solver at the next iteration
void storeImpulses(); void storeImpulses();
/// Solve the contacts /// Solve the contacts
void solve(); void solve();
/// Return true if the split impulses position correction technique is used for contacts /// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const; bool isSplitImpulseActive() const;
/// Activate or Deactivate the split impulses for contacts /// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive); void setIsSplitImpulseActive(bool isActive);
/// Activate or deactivate the solving of friction constraints at the center of /// Activate or deactivate the solving of friction constraints at the center of
/// the contact manifold instead of solving them at each contact point /// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Clean up the constraint solver /// Clean up the constraint solver
void cleanup(); void cleanup();
}; };
// Set the split velocities arrays // Set the split velocities arrays
inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities, inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
Vector3* splitAngularVelocities) { Vector3* splitAngularVelocities) {
assert(splitLinearVelocities != NULL); assert(splitLinearVelocities != NULL);
assert(splitAngularVelocities != NULL); assert(splitAngularVelocities != NULL);
mSplitLinearVelocities = splitLinearVelocities; mSplitLinearVelocities = splitLinearVelocities;
mSplitAngularVelocities = splitAngularVelocities; mSplitAngularVelocities = splitAngularVelocities;
} }
// Set the constrained velocities arrays // Set the constrained velocities arrays
inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities) { Vector3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL); assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL); assert(constrainedAngularVelocities != NULL);
mLinearVelocities = constrainedLinearVelocities; mLinearVelocities = constrainedLinearVelocities;
mAngularVelocities = constrainedAngularVelocities; mAngularVelocities = constrainedAngularVelocities;
} }
// Return true if the split impulses position correction technique is used for contacts // Return true if the split impulses position correction technique is used for contacts
inline bool ContactSolver::isSplitImpulseActive() const { inline bool ContactSolver::isSplitImpulseActive() const {
return mIsSplitImpulseActive; return mIsSplitImpulseActive;
} }
// Activate or Deactivate the split impulses for contacts // Activate or Deactivate the split impulses for contacts
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
mIsSplitImpulseActive = isActive; mIsSplitImpulseActive = isActive;
} }
// Activate or deactivate the solving of friction constraints at the center of // Activate or deactivate the solving of friction constraints at the center of
// the contact manifold instead of solving them at each contact point // the contact manifold instead of solving them at each contact point
inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
mIsSolveFrictionAtContactManifoldCenterActive = isActive; mIsSolveFrictionAtContactManifoldCenterActive = isActive;
} }
// Compute the collision restitution factor from the restitution factor of each body // Compute the collision restitution factor from the restitution factor of each body
inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, inline float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
RigidBody* body2) const { RigidBody* body2) const {
decimal restitution1 = body1->getMaterial().getBounciness(); float restitution1 = body1->getMaterial().getBounciness();
decimal restitution2 = body2->getMaterial().getBounciness(); float restitution2 = body2->getMaterial().getBounciness();
// Return the largest restitution factor // Return the largest restitution factor
return (restitution1 > restitution2) ? restitution1 : restitution2; return (restitution1 > restitution2) ? restitution1 : restitution2;
} }
// Compute the mixed friction coefficient from the friction coefficient of each body // Compute the mixed friction coefficient from the friction coefficient of each body
inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, inline float ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
RigidBody *body2) const { RigidBody *body2) const {
// Use the geometric mean to compute the mixed friction coefficient // Use the geometric mean to compute the mixed friction coefficient
return sqrt(body1->getMaterial().getFrictionCoefficient() * return sqrt(body1->getMaterial().getFrictionCoefficient() *
body2->getMaterial().getFrictionCoefficient()); body2->getMaterial().getFrictionCoefficient());
} }
// Compute th mixed rolling resistance factor between two bodies // Compute th mixed rolling resistance factor between two bodies
inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, inline float ContactSolver::computeMixedRollingResistance(RigidBody* body1,
RigidBody* body2) const { RigidBody* body2) const {
return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); return float(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance());
} }
// Compute a penetration constraint impulse // Compute a penetration constraint impulse
inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda, inline const Impulse ContactSolver::computePenetrationImpulse(float deltaLambda,
const ContactPointSolver& contactPoint) const ContactPointSolver& contactPoint)
const { const {
return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
} }
// Compute the first friction constraint impulse // Compute the first friction constraint impulse
inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, inline const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const ContactPointSolver& contactPoint)
const { const {
return Impulse(-contactPoint.frictionVector1 * deltaLambda, return Impulse(-contactPoint.frictionVector1 * deltaLambda,
-contactPoint.r1CrossT1 * deltaLambda, -contactPoint.r1CrossT1 * deltaLambda,
contactPoint.frictionVector1 * deltaLambda, contactPoint.frictionVector1 * deltaLambda,
contactPoint.r2CrossT1 * deltaLambda); contactPoint.r2CrossT1 * deltaLambda);
} }
// Compute the second friction constraint impulse // Compute the second friction constraint impulse
inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, inline const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const ContactPointSolver& contactPoint)
const { const {
return Impulse(-contactPoint.frictionVector2 * deltaLambda, return Impulse(-contactPoint.frictionVector2 * deltaLambda,
-contactPoint.r1CrossT2 * deltaLambda, -contactPoint.r1CrossT2 * deltaLambda,
contactPoint.frictionVector2 * deltaLambda, contactPoint.frictionVector2 * deltaLambda,
contactPoint.r2CrossT2 * deltaLambda); contactPoint.r2CrossT2 * deltaLambda);
} }
} }
#endif

1203
ephysics/engine/DynamicsWorld.cpp
View File

File diff suppressed because it is too large Load Diff

500
ephysics/engine/DynamicsWorld.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_DYNAMICS_WORLD_H
#define REACTPHYSICS3D_DYNAMICS_WORLD_H
// Libraries // Libraries
#include <ephysics/engine/CollisionWorld.h> #include <ephysics/engine/CollisionWorld.h>
@ -46,300 +25,300 @@ namespace reactphysics3d {
*/ */
class DynamicsWorld : public CollisionWorld { class DynamicsWorld : public CollisionWorld {
protected : protected :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Contact solver /// Contact solver
ContactSolver mContactSolver; ContactSolver mContactSolver;
/// Constraint solver /// Constraint solver
ConstraintSolver mConstraintSolver; ConstraintSolver mConstraintSolver;
/// Number of iterations for the velocity solver of the Sequential Impulses technique /// Number of iterations for the velocity solver of the Sequential Impulses technique
uint mNbVelocitySolverIterations; uint32_t mNbVelocitySolverIterations;
/// Number of iterations for the position solver of the Sequential Impulses technique /// Number of iterations for the position solver of the Sequential Impulses technique
uint mNbPositionSolverIterations; uint32_t mNbPositionSolverIterations;
/// True if the spleeping technique for inactive bodies is enabled /// True if the spleeping technique for inactive bodies is enabled
bool mIsSleepingEnabled; bool mIsSleepingEnabled;
/// All the rigid bodies of the physics world /// All the rigid bodies of the physics world
std::set<RigidBody*> mRigidBodies; std::set<RigidBody*> mRigidBodies;
/// All the joints of the world /// All the joints of the world
std::set<Joint*> mJoints; std::set<Joint*> mJoints;
/// Gravity vector of the world /// Gravity vector of the world
Vector3 mGravity; Vector3 mGravity;
/// Current frame time step (in seconds) /// Current frame time step (in seconds)
decimal mTimeStep; float mTimeStep;
/// True if the gravity force is on /// True if the gravity force is on
bool mIsGravityEnabled; bool mIsGravityEnabled;
/// Array of constrained linear velocities (state of the linear velocities /// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints) /// after solving the constraints)
Vector3* mConstrainedLinearVelocities; Vector3* mConstrainedLinearVelocities;
/// Array of constrained angular velocities (state of the angular velocities /// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints) /// after solving the constraints)
Vector3* mConstrainedAngularVelocities; Vector3* mConstrainedAngularVelocities;
/// Split linear velocities for the position contact solver (split impulse) /// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities; Vector3* mSplitLinearVelocities;
/// Split angular velocities for the position contact solver (split impulse) /// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities; Vector3* mSplitAngularVelocities;
/// Array of constrained rigid bodies position (for position error correction) /// Array of constrained rigid bodies position (for position error correction)
Vector3* mConstrainedPositions; Vector3* mConstrainedPositions;
/// Array of constrained rigid bodies orientation (for position error correction) /// Array of constrained rigid bodies orientation (for position error correction)
Quaternion* mConstrainedOrientations; Quaternion* mConstrainedOrientations;
/// Map body to their index in the constrained velocities array /// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex; std::map<RigidBody*, uint32_t> mMapBodyToConstrainedVelocityIndex;
/// Number of islands in the world /// Number of islands in the world
uint mNbIslands; uint32_t mNbIslands;
/// Current allocated capacity for the islands /// Current allocated capacity for the islands
uint mNbIslandsCapacity; uint32_t mNbIslandsCapacity;
/// Array with all the islands of awaken bodies /// Array with all the islands of awaken bodies
Island** mIslands; Island** mIslands;
/// Current allocated capacity for the bodies /// Current allocated capacity for the bodies
uint mNbBodiesCapacity; uint32_t mNbBodiesCapacity;
/// Sleep linear velocity threshold /// Sleep linear velocity threshold
decimal mSleepLinearVelocity; float mSleepLinearVelocity;
/// Sleep angular velocity threshold /// Sleep angular velocity threshold
decimal mSleepAngularVelocity; float mSleepAngularVelocity;
/// Time (in seconds) before a body is put to sleep if its velocity /// Time (in seconds) before a body is put to sleep if its velocity
/// becomes smaller than the sleep velocity. /// becomes smaller than the sleep velocity.
decimal mTimeBeforeSleep; float mTimeBeforeSleep;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
DynamicsWorld(const DynamicsWorld& world); DynamicsWorld(const DynamicsWorld& world);
/// Private assignment operator /// Private assignment operator
DynamicsWorld& operator=(const DynamicsWorld& world); DynamicsWorld& operator=(const DynamicsWorld& world);
/// Integrate the positions and orientations of rigid bodies. /// Integrate the positions and orientations of rigid bodies.
void integrateRigidBodiesPositions(); void int32_tegrateRigidBodiesPositions();
/// Update the AABBs of the bodies /// Update the AABBs of the bodies
void updateRigidBodiesAABB(); void updateRigidBodiesAABB();
/// Reset the external force and torque applied to the bodies /// Reset the external force and torque applied to the bodies
void resetBodiesForceAndTorque(); void resetBodiesForceAndTorque();
/// Update the position and orientation of a body /// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
Vector3 newAngVelocity); Vector3 newAngVelocity);
/// Compute and set the interpolation factor to all bodies /// Compute and set the int32_terpolation factor to all bodies
void setInterpolationFactorToAllBodies(); void setInterpolationFactorToAllBodies();
/// Initialize the bodies velocities arrays for the next simulation step. /// Initialize the bodies velocities arrays for the next simulation step.
void initVelocityArrays(); void initVelocityArrays();
/// Integrate the velocities of rigid bodies. /// Integrate the velocities of rigid bodies.
void integrateRigidBodiesVelocities(); void int32_tegrateRigidBodiesVelocities();
/// Solve the contacts and constraints /// Solve the contacts and constraints
void solveContactsAndConstraints(); void solveContactsAndConstraints();
/// Solve the position error correction of the constraints /// Solve the position error correction of the constraints
void solvePositionCorrection(); void solvePositionCorrection();
/// Cleanup the constrained velocities array at each step /// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray(); void cleanupConstrainedVelocitiesArray();
/// Compute the islands of awake bodies. /// Compute the islands of awake bodies.
void computeIslands(); void computeIslands();
/// Update the postion/orientation of the bodies /// Update the postion/orientation of the bodies
void updateBodiesState(); void updateBodiesState();
/// Put bodies to sleep if needed. /// Put bodies to sleep if needed.
void updateSleepingBodies(); void updateSleepingBodies();
/// Add the joint to the list of joints of the two bodies involved in the joint /// Add the joint to the list of joints of the two bodies involved in the joint
void addJointToBody(Joint* joint); void addJointToBody(Joint* joint);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
DynamicsWorld(const Vector3& mGravity); DynamicsWorld(const Vector3& mGravity);
/// Destructor /// Destructor
virtual ~DynamicsWorld(); virtual ~DynamicsWorld();
/// Update the physics simulation /// Update the physics simulation
void update(decimal timeStep); void update(float timeStep);
/// Get the number of iterations for the velocity constraint solver /// Get the number of iterations for the velocity constraint solver
uint getNbIterationsVelocitySolver() const; uint32_t getNbIterationsVelocitySolver() const;
/// Set the number of iterations for the velocity constraint solver /// Set the number of iterations for the velocity constraint solver
void setNbIterationsVelocitySolver(uint nbIterations); void setNbIterationsVelocitySolver(uint32_t nbIterations);
/// Get the number of iterations for the position constraint solver /// Get the number of iterations for the position constraint solver
uint getNbIterationsPositionSolver() const; uint32_t getNbIterationsPositionSolver() const;
/// Set the number of iterations for the position constraint solver /// Set the number of iterations for the position constraint solver
void setNbIterationsPositionSolver(uint nbIterations); void setNbIterationsPositionSolver(uint32_t nbIterations);
/// Set the position correction technique used for contacts /// Set the position correction technique used for contacts
void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique); void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique);
/// Set the position correction technique used for joints /// Set the position correction technique used for joints
void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique); void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique);
/// Activate or deactivate the solving of friction constraints at the center of /// Activate or deactivate the solving of friction constraints at the center of
/// the contact manifold instead of solving them at each contact point /// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Create a rigid body into the physics world. /// Create a rigid body int32_to the physics world.
RigidBody* createRigidBody(const Transform& transform); RigidBody* createRigidBody(const Transform& transform);
/// Destroy a rigid body and all the joints which it belongs /// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody); void destroyRigidBody(RigidBody* rigidBody);
/// Create a joint between two bodies in the world and return a pointer to the new joint /// Create a joint between two bodies in the world and return a pointer to the new joint
Joint* createJoint(const JointInfo& jointInfo); Joint* createJoint(const JointInfo& jointInfo);
/// Destroy a joint /// Destroy a joint
void destroyJoint(Joint* joint); void destroyJoint(Joint* joint);
/// Return the gravity vector of the world /// Return the gravity vector of the world
Vector3 getGravity() const; Vector3 getGravity() const;
/// Set the gravity vector of the world /// Set the gravity vector of the world
void setGravity(Vector3& gravity); void setGravity(Vector3& gravity);
/// Return if the gravity is on /// Return if the gravity is on
bool isGravityEnabled() const; bool isGravityEnabled() const;
/// Enable/Disable the gravity /// Enable/Disable the gravity
void setIsGratityEnabled(bool isGravityEnabled); void setIsGratityEnabled(bool isGravityEnabled);
/// Return the number of rigid bodies in the world /// Return the number of rigid bodies in the world
uint getNbRigidBodies() const; uint32_t getNbRigidBodies() const;
/// Return the number of joints in the world /// Return the number of joints in the world
uint getNbJoints() const; uint32_t getNbJoints() const;
/// Return an iterator to the beginning of the rigid bodies of the physics world /// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator(); std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
/// Return an iterator to the end of the rigid bodies of the physics world /// Return an iterator to the end of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesEndIterator(); std::set<RigidBody*>::iterator getRigidBodiesEndIterator();
/// Return true if the sleeping technique is enabled /// Return true if the sleeping technique is enabled
bool isSleepingEnabled() const; bool isSleepingEnabled() const;
/// Enable/Disable the sleeping technique /// Enable/Disable the sleeping technique
void enableSleeping(bool isSleepingEnabled); void enableSleeping(bool isSleepingEnabled);
/// Return the current sleep linear velocity /// Return the current sleep linear velocity
decimal getSleepLinearVelocity() const; float getSleepLinearVelocity() const;
/// Set the sleep linear velocity. /// Set the sleep linear velocity.
void setSleepLinearVelocity(decimal sleepLinearVelocity); void setSleepLinearVelocity(float sleepLinearVelocity);
/// Return the current sleep angular velocity /// Return the current sleep angular velocity
decimal getSleepAngularVelocity() const; float getSleepAngularVelocity() const;
/// Set the sleep angular velocity. /// Set the sleep angular velocity.
void setSleepAngularVelocity(decimal sleepAngularVelocity); void setSleepAngularVelocity(float sleepAngularVelocity);
/// Return the time a body is required to stay still before sleeping /// Return the time a body is required to stay still before sleeping
decimal getTimeBeforeSleep() const; float getTimeBeforeSleep() const;
/// Set the time a body is required to stay still before sleeping /// Set the time a body is required to stay still before sleeping
void setTimeBeforeSleep(decimal timeBeforeSleep); void setTimeBeforeSleep(float timeBeforeSleep);
/// Set an event listener object to receive events callbacks. /// Set an event listener object to receive events callbacks.
void setEventListener(EventListener* eventListener); void setEventListener(EventListener* eventListener);
/// Test and report collisions between a given shape and all the others /// Test and report collisions between a given shape and all the others
/// shapes of the world /// shapes of the world
virtual void testCollision(const ProxyShape* shape, virtual void testCollision(const ProxyShape* shape,
CollisionCallback* callback); CollisionCallback* callback);
/// Test and report collisions between two given shapes /// Test and report collisions between two given shapes
virtual void testCollision(const ProxyShape* shape1, virtual void testCollision(const ProxyShape* shape1,
const ProxyShape* shape2, const ProxyShape* shape2,
CollisionCallback* callback); CollisionCallback* callback);
/// Test and report collisions between a body and all /// Test and report collisions between a body and all
/// the others bodies of the world /// the others bodies of the world
virtual void testCollision(const CollisionBody* body, virtual void testCollision(const CollisionBody* body,
CollisionCallback* callback); CollisionCallback* callback);
/// Test and report collisions between two bodies /// Test and report collisions between two bodies
virtual void testCollision(const CollisionBody* body1, virtual void testCollision(const CollisionBody* body1,
const CollisionBody* body2, const CollisionBody* body2,
CollisionCallback* callback); CollisionCallback* callback);
/// Test and report collisions between all shapes of the world /// Test and report collisions between all shapes of the world
virtual void testCollision(CollisionCallback* callback); virtual void testCollision(CollisionCallback* callback);
/// Return the list of all contacts of the world /// Return the list of all contacts of the world
std::vector<const ContactManifold*> getContactsList() const; std::vector<const ContactManifold*> getContactsList() const;
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class RigidBody; friend class RigidBody;
}; };
// Reset the external force and torque applied to the bodies // Reset the external force and torque applied to the bodies
inline void DynamicsWorld::resetBodiesForceAndTorque() { inline void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world // For each body of the world
std::set<RigidBody*>::iterator it; std::set<RigidBody*>::iterator it;
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
(*it)->mExternalForce.setToZero(); (*it)->mExternalForce.setToZero();
(*it)->mExternalTorque.setToZero(); (*it)->mExternalTorque.setToZero();
} }
} }
// Get the number of iterations for the velocity constraint solver // Get the number of iterations for the velocity constraint solver
inline uint DynamicsWorld::getNbIterationsVelocitySolver() const { inline uint32_t DynamicsWorld::getNbIterationsVelocitySolver() const {
return mNbVelocitySolverIterations; return mNbVelocitySolverIterations;
} }
// Set the number of iterations for the velocity constraint solver // Set the number of iterations for the velocity constraint solver
/** /**
* @param nbIterations Number of iterations for the velocity solver * @param nbIterations Number of iterations for the velocity solver
*/ */
inline void DynamicsWorld::setNbIterationsVelocitySolver(uint nbIterations) { inline void DynamicsWorld::setNbIterationsVelocitySolver(uint32_t nbIterations) {
mNbVelocitySolverIterations = nbIterations; mNbVelocitySolverIterations = nbIterations;
} }
// Get the number of iterations for the position constraint solver // Get the number of iterations for the position constraint solver
inline uint DynamicsWorld::getNbIterationsPositionSolver() const { inline uint32_t DynamicsWorld::getNbIterationsPositionSolver() const {
return mNbPositionSolverIterations; return mNbPositionSolverIterations;
} }
// Set the number of iterations for the position constraint solver // Set the number of iterations for the position constraint solver
/** /**
* @param nbIterations Number of iterations for the position solver * @param nbIterations Number of iterations for the position solver
*/ */
inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) { inline void DynamicsWorld::setNbIterationsPositionSolver(uint32_t nbIterations) {
mNbPositionSolverIterations = nbIterations; mNbPositionSolverIterations = nbIterations;
} }
// Set the position correction technique used for contacts // Set the position correction technique used for contacts
@ -347,13 +326,13 @@ inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) {
* @param technique Technique used for the position correction (Baumgarte or Split Impulses) * @param technique Technique used for the position correction (Baumgarte or Split Impulses)
*/ */
inline void DynamicsWorld::setContactsPositionCorrectionTechnique( inline void DynamicsWorld::setContactsPositionCorrectionTechnique(
ContactsPositionCorrectionTechnique technique) { ContactsPositionCorrectionTechnique technique) {
if (technique == BAUMGARTE_CONTACTS) { if (technique == BAUMGARTE_CONTACTS) {
mContactSolver.setIsSplitImpulseActive(false); mContactSolver.setIsSplitImpulseActive(false);
} }
else { else {
mContactSolver.setIsSplitImpulseActive(true); mContactSolver.setIsSplitImpulseActive(true);
} }
} }
// Set the position correction technique used for joints // Set the position correction technique used for joints
@ -361,23 +340,23 @@ inline void DynamicsWorld::setContactsPositionCorrectionTechnique(
* @param technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel) * @param technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel)
*/ */
inline void DynamicsWorld::setJointsPositionCorrectionTechnique( inline void DynamicsWorld::setJointsPositionCorrectionTechnique(
JointsPositionCorrectionTechnique technique) { JointsPositionCorrectionTechnique technique) {
if (technique == BAUMGARTE_JOINTS) { if (technique == BAUMGARTE_JOINTS) {
mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false); mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
} }
else { else {
mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true); mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
} }
} }
// Activate or deactivate the solving of friction constraints at the center of // Activate or deactivate the solving of friction constraints at the center of
// the contact manifold instead of solving them at each contact point // the contact manifold instead of solving them at each contact point
/** /**
* @param isActive True if you want the friction to be solved at the center of * @param isActive True if you want the friction to be solved at the center of
* the contact manifold and false otherwise * the contact manifold and false otherwise
*/ */
inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
} }
// Return the gravity vector of the world // Return the gravity vector of the world
@ -385,7 +364,7 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool
* @return The current gravity vector (in meter per seconds squared) * @return The current gravity vector (in meter per seconds squared)
*/ */
inline Vector3 DynamicsWorld::getGravity() const { inline Vector3 DynamicsWorld::getGravity() const {
return mGravity; return mGravity;
} }
// Set the gravity vector of the world // Set the gravity vector of the world
@ -393,7 +372,7 @@ inline Vector3 DynamicsWorld::getGravity() const {
* @param gravity The gravity vector (in meter per seconds squared) * @param gravity The gravity vector (in meter per seconds squared)
*/ */
inline void DynamicsWorld::setGravity(Vector3& gravity) { inline void DynamicsWorld::setGravity(Vector3& gravity) {
mGravity = gravity; mGravity = gravity;
} }
// Return if the gravity is enaled // Return if the gravity is enaled
@ -401,32 +380,32 @@ inline void DynamicsWorld::setGravity(Vector3& gravity) {
* @return True if the gravity is enabled in the world * @return True if the gravity is enabled in the world
*/ */
inline bool DynamicsWorld::isGravityEnabled() const { inline bool DynamicsWorld::isGravityEnabled() const {
return mIsGravityEnabled; return mIsGravityEnabled;
} }
// Enable/Disable the gravity // Enable/Disable the gravity
/** /**
* @param isGravityEnabled True if you want to enable the gravity in the world * @param isGravityEnabled True if you want to enable the gravity in the world
* and false otherwise * and false otherwise
*/ */
inline void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) { inline void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) {
mIsGravityEnabled = isGravityEnabled; mIsGravityEnabled = isGravityEnabled;
} }
// Return the number of rigid bodies in the world // Return the number of rigid bodies in the world
/** /**
* @return Number of rigid bodies in the world * @return Number of rigid bodies in the world
*/ */
inline uint DynamicsWorld::getNbRigidBodies() const { inline uint32_t DynamicsWorld::getNbRigidBodies() const {
return mRigidBodies.size(); return mRigidBodies.size();
} }
/// Return the number of joints in the world /// Return the number of joints in the world
/** /**
* @return Number of joints in the world * @return Number of joints in the world
*/ */
inline uint DynamicsWorld::getNbJoints() const { inline uint32_t DynamicsWorld::getNbJoints() const {
return mJoints.size(); return mJoints.size();
} }
// Return an iterator to the beginning of the bodies of the physics world // Return an iterator to the beginning of the bodies of the physics world
@ -434,7 +413,7 @@ inline uint DynamicsWorld::getNbJoints() const {
* @return Starting iterator of the set of rigid bodies * @return Starting iterator of the set of rigid bodies
*/ */
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() { inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
return mRigidBodies.begin(); return mRigidBodies.begin();
} }
// Return an iterator to the end of the bodies of the physics world // Return an iterator to the end of the bodies of the physics world
@ -442,7 +421,7 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator
* @return Ending iterator of the set of rigid bodies * @return Ending iterator of the set of rigid bodies
*/ */
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator() { inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator() {
return mRigidBodies.end(); return mRigidBodies.end();
} }
// Return true if the sleeping technique is enabled // Return true if the sleeping technique is enabled
@ -450,15 +429,15 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
* @return True if the sleeping technique is enabled and false otherwise * @return True if the sleeping technique is enabled and false otherwise
*/ */
inline bool DynamicsWorld::isSleepingEnabled() const { inline bool DynamicsWorld::isSleepingEnabled() const {
return mIsSleepingEnabled; return mIsSleepingEnabled;
} }
// Return the current sleep linear velocity // Return the current sleep linear velocity
/** /**
* @return The sleep linear velocity (in meters per second) * @return The sleep linear velocity (in meters per second)
*/ */
inline decimal DynamicsWorld::getSleepLinearVelocity() const { inline float DynamicsWorld::getSleepLinearVelocity() const {
return mSleepLinearVelocity; return mSleepLinearVelocity;
} }
// Set the sleep linear velocity. // Set the sleep linear velocity.
@ -468,17 +447,17 @@ inline decimal DynamicsWorld::getSleepLinearVelocity() const {
/** /**
* @param sleepLinearVelocity The sleep linear velocity (in meters per second) * @param sleepLinearVelocity The sleep linear velocity (in meters per second)
*/ */
inline void DynamicsWorld::setSleepLinearVelocity(decimal sleepLinearVelocity) { inline void DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) {
assert(sleepLinearVelocity >= decimal(0.0)); assert(sleepLinearVelocity >= float(0.0));
mSleepLinearVelocity = sleepLinearVelocity; mSleepLinearVelocity = sleepLinearVelocity;
} }
// Return the current sleep angular velocity // Return the current sleep angular velocity
/** /**
* @return The sleep angular velocity (in radian per second) * @return The sleep angular velocity (in radian per second)
*/ */
inline decimal DynamicsWorld::getSleepAngularVelocity() const { inline float DynamicsWorld::getSleepAngularVelocity() const {
return mSleepAngularVelocity; return mSleepAngularVelocity;
} }
// Set the sleep angular velocity. // Set the sleep angular velocity.
@ -488,17 +467,17 @@ inline decimal DynamicsWorld::getSleepAngularVelocity() const {
/** /**
* @param sleepAngularVelocity The sleep angular velocity (in radian per second) * @param sleepAngularVelocity The sleep angular velocity (in radian per second)
*/ */
inline void DynamicsWorld::setSleepAngularVelocity(decimal sleepAngularVelocity) { inline void DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) {
assert(sleepAngularVelocity >= decimal(0.0)); assert(sleepAngularVelocity >= float(0.0));
mSleepAngularVelocity = sleepAngularVelocity; mSleepAngularVelocity = sleepAngularVelocity;
} }
// Return the time a body is required to stay still before sleeping // Return the time a body is required to stay still before sleeping
/** /**
* @return Time a body is required to stay still before sleeping (in seconds) * @return Time a body is required to stay still before sleeping (in seconds)
*/ */
inline decimal DynamicsWorld::getTimeBeforeSleep() const { inline float DynamicsWorld::getTimeBeforeSleep() const {
return mTimeBeforeSleep; return mTimeBeforeSleep;
} }
@ -506,22 +485,21 @@ inline decimal DynamicsWorld::getTimeBeforeSleep() const {
/** /**
* @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds) * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds)
*/ */
inline void DynamicsWorld::setTimeBeforeSleep(decimal timeBeforeSleep) { inline void DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) {
assert(timeBeforeSleep >= decimal(0.0)); assert(timeBeforeSleep >= float(0.0));
mTimeBeforeSleep = timeBeforeSleep; mTimeBeforeSleep = timeBeforeSleep;
} }
// Set an event listener object to receive events callbacks. // Set an event listener object to receive events callbacks.
/// If you use NULL as an argument, the events callbacks will be disabled. /// If you use NULL as an argument, the events callbacks will be disabled.
/** /**
* @param eventListener Pointer to the event listener object that will receive * @param eventListener Pointer to the event listener object that will receive
* event callbacks during the simulation * event callbacks during the simulation
*/ */
inline void DynamicsWorld::setEventListener(EventListener* eventListener) { inline void DynamicsWorld::setEventListener(EventListener* eventListener) {
mEventListener = eventListener; mEventListener = eventListener;
} }
} }
#endif

85
ephysics/engine/EventListener.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_EVENT_LISTENER_H
#define REACTPHYSICS3D_EVENT_LISTENER_H
// Libraries // Libraries
#include <ephysics/constraint/ContactPoint.h> #include <ephysics/constraint/ContactPoint.h>
@ -41,39 +20,37 @@ namespace reactphysics3d {
*/ */
class EventListener { class EventListener {
public : public :
/// Constructor /// Constructor
EventListener() {} EventListener() {}
/// Destructor /// Destructor
virtual ~EventListener() {} virtual ~EventListener() {}
/// Called when a new contact point is found between two bodies that were separated before /// Called when a new contact point is found between two bodies that were separated before
/** /**
* @param contact Information about the contact * @param contact Information about the contact
*/ */
virtual void beginContact(const ContactPointInfo& contact) {}; virtual void beginContact(const ContactPointInfo& contact) {};
/// Called when a new contact point is found between two bodies /// Called when a new contact point is found between two bodies
/** /**
* @param contact Information about the contact * @param contact Information about the contact
*/ */
virtual void newContact(const ContactPointInfo& contact) {} virtual void newContact(const ContactPointInfo& contact) {}
/// Called at the beginning of an internal tick of the simulation step. /// Called at the beginning of an int32_ternal tick of the simulation step.
/// Each time the DynamicsWorld::update() method is called, the physics /// Each time the DynamicsWorld::update() method is called, the physics
/// engine will do several internal simulation steps. This method is /// engine will do several int32_ternal simulation steps. This method is
/// called at the beginning of each internal simulation step. /// called at the beginning of each int32_ternal simulation step.
virtual void beginInternalTick() {} virtual void beginInternalTick() {}
/// Called at the end of an internal tick of the simulation step. /// Called at the end of an int32_ternal tick of the simulation step.
/// Each time the DynamicsWorld::update() metho is called, the physics /// Each time the DynamicsWorld::update() metho is called, the physics
/// engine will do several internal simulation steps. This method is /// engine will do several int32_ternal simulation steps. This method is
/// called at the end of each internal simulation step. /// called at the end of each int32_ternal simulation step.
virtual void endInternalTick() {} virtual void endInternalTick() {}
}; };
} }
#endif

95
ephysics/engine/Impulse.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_IMPULSE_H
#define REACTPHYSICS3D_IMPULSE_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -37,51 +16,49 @@ namespace reactphysics3d {
*/ */
struct Impulse { struct Impulse {
private: private:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private assignment operator /// Private assignment operator
Impulse& operator=(const Impulse& impulse); Impulse& operator=(const Impulse& impulse);
public: public:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Linear impulse applied to the first body /// Linear impulse applied to the first body
const Vector3 linearImpulseBody1; const Vector3 linearImpulseBody1;
/// Angular impulse applied to the first body /// Angular impulse applied to the first body
const Vector3 angularImpulseBody1; const Vector3 angularImpulseBody1;
/// Linear impulse applied to the second body /// Linear impulse applied to the second body
const Vector3 linearImpulseBody2; const Vector3 linearImpulseBody2;
/// Angular impulse applied to the second body /// Angular impulse applied to the second body
const Vector3 angularImpulseBody2; const Vector3 angularImpulseBody2;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1, Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1,
const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2) const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2)
: linearImpulseBody1(initLinearImpulseBody1), : linearImpulseBody1(initLinearImpulseBody1),
angularImpulseBody1(initAngularImpulseBody1), angularImpulseBody1(initAngularImpulseBody1),
linearImpulseBody2(initLinearImpulseBody2), linearImpulseBody2(initLinearImpulseBody2),
angularImpulseBody2(initAngularImpulseBody2) { angularImpulseBody2(initAngularImpulseBody2) {
} }
/// Copy-constructor /// Copy-constructor
Impulse(const Impulse& impulse) Impulse(const Impulse& impulse)
: linearImpulseBody1(impulse.linearImpulseBody1), : linearImpulseBody1(impulse.linearImpulseBody1),
angularImpulseBody1(impulse.angularImpulseBody1), angularImpulseBody1(impulse.angularImpulseBody1),
linearImpulseBody2(impulse.linearImpulseBody2), linearImpulseBody2(impulse.linearImpulseBody2),
angularImpulseBody2(impulse.angularImpulseBody2) { angularImpulseBody2(impulse.angularImpulseBody2) {
; ;
} }
}; };
} }
#endif

61
ephysics/engine/Island.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/engine/Island.h> #include <ephysics/engine/Island.h>
@ -29,26 +10,26 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, Island::Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints,
MemoryAllocator& memoryAllocator) MemoryAllocator& memoryAllocator)
: mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0), : mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0),
mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {
// Allocate memory for the arrays // Allocate memory for the arrays
mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies; mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies;
mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies); mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies);
mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds; mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds;
mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate( mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate(
mNbAllocatedBytesContactManifolds); mNbAllocatedBytesContactManifolds);
mNbAllocatedBytesJoints = sizeof(Joint*) * nbMaxJoints; mNbAllocatedBytesJoints = sizeof(Joint*) * nbMaxJoints;
mJoints = (Joint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints); mJoints = (Joint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints);
} }
// Destructor // Destructor
Island::~Island() { Island::~Island() {
// Release the memory of the arrays // Release the memory of the arrays
mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies); mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies);
mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds); mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds);
mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints); mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints);
} }

181
ephysics/engine/Island.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_ISLAND_H
#define REACTPHYSICS3D_ISLAND_H
// Libraries // Libraries
#include <ephysics/memory/MemoryAllocator.h> #include <ephysics/memory/MemoryAllocator.h>
@ -41,140 +20,138 @@ namespace reactphysics3d {
*/ */
class Island { class Island {
private: private:
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Array with all the bodies of the island /// Array with all the bodies of the island
RigidBody** mBodies; RigidBody** mBodies;
/// Array with all the contact manifolds between bodies of the island /// Array with all the contact manifolds between bodies of the island
ContactManifold** mContactManifolds; ContactManifold** mContactManifolds;
/// Array with all the joints between bodies of the island /// Array with all the joints between bodies of the island
Joint** mJoints; Joint** mJoints;
/// Current number of bodies in the island /// Current number of bodies in the island
uint mNbBodies; uint32_t mNbBodies;
/// Current number of contact manifold in the island /// Current number of contact manifold in the island
uint mNbContactManifolds; uint32_t mNbContactManifolds;
/// Current number of joints in the island /// Current number of joints in the island
uint mNbJoints; uint32_t mNbJoints;
/// Reference to the memory allocator /// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator; MemoryAllocator& mMemoryAllocator;
/// Number of bytes allocated for the bodies array /// Number of bytes allocated for the bodies array
size_t mNbAllocatedBytesBodies; size_t mNbAllocatedBytesBodies;
/// Number of bytes allocated for the contact manifolds array /// Number of bytes allocated for the contact manifolds array
size_t mNbAllocatedBytesContactManifolds; size_t mNbAllocatedBytesContactManifolds;
/// Number of bytes allocated for the joints array /// Number of bytes allocated for the joints array
size_t mNbAllocatedBytesJoints; size_t mNbAllocatedBytesJoints;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private assignment operator /// Private assignment operator
Island& operator=(const Island& island); Island& operator=(const Island& island);
/// Private copy-constructor /// Private copy-constructor
Island(const Island& island); Island(const Island& island);
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints,
MemoryAllocator& memoryAllocator); MemoryAllocator& memoryAllocator);
/// Destructor /// Destructor
~Island(); ~Island();
/// Add a body into the island /// Add a body int32_to the island
void addBody(RigidBody* body); void addBody(RigidBody* body);
/// Add a contact manifold into the island /// Add a contact manifold int32_to the island
void addContactManifold(ContactManifold* contactManifold); void addContactManifold(ContactManifold* contactManifold);
/// Add a joint into the island /// Add a joint int32_to the island
void addJoint(Joint* joint); void addJoint(Joint* joint);
/// Return the number of bodies in the island /// Return the number of bodies in the island
uint getNbBodies() const; uint32_t getNbBodies() const;
/// Return the number of contact manifolds in the island /// Return the number of contact manifolds in the island
uint getNbContactManifolds() const; uint32_t getNbContactManifolds() const;
/// Return the number of joints in the island /// Return the number of joints in the island
uint getNbJoints() const; uint32_t getNbJoints() const;
/// Return a pointer to the array of bodies /// Return a pointer to the array of bodies
RigidBody** getBodies(); RigidBody** getBodies();
/// Return a pointer to the array of contact manifolds /// Return a pointer to the array of contact manifolds
ContactManifold** getContactManifold(); ContactManifold** getContactManifold();
/// Return a pointer to the array of joints /// Return a pointer to the array of joints
Joint** getJoints(); Joint** getJoints();
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;
}; };
// Add a body into the island // Add a body int32_to the island
inline void Island::addBody(RigidBody* body) { inline void Island::addBody(RigidBody* body) {
assert(!body->isSleeping()); assert(!body->isSleeping());
mBodies[mNbBodies] = body; mBodies[mNbBodies] = body;
mNbBodies++; mNbBodies++;
} }
// Add a contact manifold into the island // Add a contact manifold int32_to the island
inline void Island::addContactManifold(ContactManifold* contactManifold) { inline void Island::addContactManifold(ContactManifold* contactManifold) {
mContactManifolds[mNbContactManifolds] = contactManifold; mContactManifolds[mNbContactManifolds] = contactManifold;
mNbContactManifolds++; mNbContactManifolds++;
} }
// Add a joint into the island // Add a joint int32_to the island
inline void Island::addJoint(Joint* joint) { inline void Island::addJoint(Joint* joint) {
mJoints[mNbJoints] = joint; mJoints[mNbJoints] = joint;
mNbJoints++; mNbJoints++;
} }
// Return the number of bodies in the island // Return the number of bodies in the island
inline uint Island::getNbBodies() const { inline uint32_t Island::getNbBodies() const {
return mNbBodies; return mNbBodies;
} }
// Return the number of contact manifolds in the island // Return the number of contact manifolds in the island
inline uint Island::getNbContactManifolds() const { inline uint32_t Island::getNbContactManifolds() const {
return mNbContactManifolds; return mNbContactManifolds;
} }
// Return the number of joints in the island // Return the number of joints in the island
inline uint Island::getNbJoints() const { inline uint32_t Island::getNbJoints() const {
return mNbJoints; return mNbJoints;
} }
// Return a pointer to the array of bodies // Return a pointer to the array of bodies
inline RigidBody** Island::getBodies() { inline RigidBody** Island::getBodies() {
return mBodies; return mBodies;
} }
// Return a pointer to the array of contact manifolds // Return a pointer to the array of contact manifolds
inline ContactManifold** Island::getContactManifold() { inline ContactManifold** Island::getContactManifold() {
return mContactManifolds; return mContactManifolds;
} }
// Return a pointer to the array of joints // Return a pointer to the array of joints
inline Joint** Island::getJoints() { inline Joint** Island::getJoints() {
return mJoints; return mJoints;
} }
} }
#endif

39
ephysics/engine/Material.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* 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 // Libraries
#include <ephysics/engine/Material.h> #include <ephysics/engine/Material.h>
@ -30,16 +11,16 @@ using namespace reactphysics3d;
// Constructor // Constructor
Material::Material() Material::Material()
: mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT), : mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT),
mRollingResistance(DEFAULT_ROLLING_RESISTANCE), mRollingResistance(DEFAULT_ROLLING_RESISTANCE),
mBounciness(DEFAULT_BOUNCINESS) { mBounciness(DEFAULT_BOUNCINESS) {
} }
// Copy-constructor // Copy-constructor
Material::Material(const Material& material) Material::Material(const Material& material)
: mFrictionCoefficient(material.mFrictionCoefficient), : mFrictionCoefficient(material.mFrictionCoefficient),
mRollingResistance(material.mRollingResistance), mBounciness(material.mBounciness) { mRollingResistance(material.mRollingResistance), mBounciness(material.mBounciness) {
} }

141
ephysics/engine/Material.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_MATERIAL_H
#define REACTPHYSICS3D_MATERIAL_H
// Libraries // Libraries
#include <cassert> #include <cassert>
@ -40,60 +19,60 @@ namespace reactphysics3d {
*/ */
class Material { class Material {
private : private :
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Friction coefficient (positive value) /// Friction coefficient (positive value)
decimal mFrictionCoefficient; float mFrictionCoefficient;
/// Rolling resistance factor (positive value) /// Rolling resistance factor (positive value)
decimal mRollingResistance; float mRollingResistance;
/// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body /// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body
decimal mBounciness; float mBounciness;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Material(); Material();
/// Copy-constructor /// Copy-constructor
Material(const Material& material); Material(const Material& material);
/// Destructor /// Destructor
~Material(); ~Material();
/// Return the bounciness /// Return the bounciness
decimal getBounciness() const; float getBounciness() const;
/// Set the bounciness. /// Set the bounciness.
void setBounciness(decimal bounciness); void setBounciness(float bounciness);
/// Return the friction coefficient /// Return the friction coefficient
decimal getFrictionCoefficient() const; float getFrictionCoefficient() const;
/// Set the friction coefficient. /// Set the friction coefficient.
void setFrictionCoefficient(decimal frictionCoefficient); void setFrictionCoefficient(float frictionCoefficient);
/// Return the rolling resistance factor /// Return the rolling resistance factor
decimal getRollingResistance() const; float getRollingResistance() const;
/// Set the rolling resistance factor /// Set the rolling resistance factor
void setRollingResistance(decimal rollingResistance); void setRollingResistance(float rollingResistance);
/// Overloaded assignment operator /// Overloaded assignment operator
Material& operator=(const Material& material); Material& operator=(const Material& material);
}; };
// Return the bounciness // Return the bounciness
/** /**
* @return Bounciness factor (between 0 and 1) where 1 is very bouncy * @return Bounciness factor (between 0 and 1) where 1 is very bouncy
*/ */
inline decimal Material::getBounciness() const { inline float Material::getBounciness() const {
return mBounciness; return mBounciness;
} }
// Set the bounciness. // Set the bounciness.
@ -102,17 +81,17 @@ inline decimal Material::getBounciness() const {
/** /**
* @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy
*/ */
inline void Material::setBounciness(decimal bounciness) { inline void Material::setBounciness(float bounciness) {
assert(bounciness >= decimal(0.0) && bounciness <= decimal(1.0)); assert(bounciness >= float(0.0) && bounciness <= float(1.0));
mBounciness = bounciness; mBounciness = bounciness;
} }
// Return the friction coefficient // Return the friction coefficient
/** /**
* @return Friction coefficient (positive value) * @return Friction coefficient (positive value)
*/ */
inline decimal Material::getFrictionCoefficient() const { inline float Material::getFrictionCoefficient() const {
return mFrictionCoefficient; return mFrictionCoefficient;
} }
// Set the friction coefficient. // Set the friction coefficient.
@ -121,9 +100,9 @@ inline decimal Material::getFrictionCoefficient() const {
/** /**
* @param frictionCoefficient Friction coefficient (positive value) * @param frictionCoefficient Friction coefficient (positive value)
*/ */
inline void Material::setFrictionCoefficient(decimal frictionCoefficient) { inline void Material::setFrictionCoefficient(float frictionCoefficient) {
assert(frictionCoefficient >= decimal(0.0)); assert(frictionCoefficient >= float(0.0));
mFrictionCoefficient = frictionCoefficient; mFrictionCoefficient = frictionCoefficient;
} }
// Return the rolling resistance factor. If this value is larger than zero, // Return the rolling resistance factor. If this value is larger than zero,
@ -132,8 +111,8 @@ inline void Material::setFrictionCoefficient(decimal frictionCoefficient) {
/** /**
* @return The rolling resistance factor (positive value) * @return The rolling resistance factor (positive value)
*/ */
inline decimal Material::getRollingResistance() const { inline float Material::getRollingResistance() const {
return mRollingResistance; return mRollingResistance;
} }
// Set the rolling resistance factor. If this value is larger than zero, // Set the rolling resistance factor. If this value is larger than zero,
@ -142,25 +121,23 @@ inline decimal Material::getRollingResistance() const {
/** /**
* @param rollingResistance The rolling resistance factor * @param rollingResistance The rolling resistance factor
*/ */
inline void Material::setRollingResistance(decimal rollingResistance) { inline void Material::setRollingResistance(float rollingResistance) {
assert(rollingResistance >= 0); assert(rollingResistance >= 0);
mRollingResistance = rollingResistance; mRollingResistance = rollingResistance;
} }
// Overloaded assignment operator // Overloaded assignment operator
inline Material& Material::operator=(const Material& material) { inline Material& Material::operator=(const Material& material) {
// Check for self-assignment // Check for self-assignment
if (this != &material) { if (this != &material) {
mFrictionCoefficient = material.mFrictionCoefficient; mFrictionCoefficient = material.mFrictionCoefficient;
mBounciness = material.mBounciness; mBounciness = material.mBounciness;
mRollingResistance = material.mRollingResistance; mRollingResistance = material.mRollingResistance;
} }
// Return this material // Return this material
return *this; return *this;
} }
} }
#endif

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